Add timeout.

diff --git a/include/bno055_tty.h b/include/bno055_tty.h
index 117f87c..61e8688 100644 (file)
--- a/include/bno055_tty.h
+++ b/include/bno055_tty.h
@@ -55,10 +55,13 @@ extern s8 BNO055_tty_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
 */
 extern s8 BNO055_tty_init(int fd,struct bno055_t *bno055);
 
-/*     Brief : Delay routine
- *     \param : delay in ms
+/*
+ *     \Brief: TTY close routine
+ *     \Return: status of the close
+ *     \param bno055: pointer to a bno055 structure
 */
-extern void BNO055_tty_delay_ms(u32 ms);
+extern s8 BNO055_tty_close(struct bno055_t *bno055);
+
 
 #ifdef __cplusplus
 }

diff --git a/lib/bno055_tty.c b/lib/bno055_tty.c
index 3a8c463..4476599 100644 (file)
--- a/lib/bno055_tty.c
+++ b/lib/bno055_tty.c
@@ -37,19 +37,26 @@
 
 #define BNO055_UART_SUCCESS    0x01
 
+/* This makes the code non-reentrant,
+but the Bosch BNO055_driver is already non-reentrant */
+static struct termios old_tty;
+
 s8 BNO055_tty_init(int fd,struct bno055_t *bno055)
 {
        struct termios tty;
        int status;
        
        if(tcgetattr(fd,&tty)) return BNO055_ERROR;
+       old_tty=tty;
         if(cfsetspeed(&tty,B115200)) return BNO055_OUT_OF_RANGE;
        cfmakeraw(&tty);
        tty.c_cflag&=~CRTSCTS;
        tty.c_cflag|=CLOCAL;
+       tty.c_cc[VMIN]=0;
+       tty.c_cc[VTIME]=10; /* 1 s timeout */
         if(tcsetattr(fd,TCSANOW,&tty)) return BNO055_ERROR;
 
-        /* nRESET low*/
+        /* nRESET low */
        status=TIOCM_DTR | TIOCM_RTS;
        ioctl(fd,TIOCMBIS,&status);
        usleep(1);
@@ -67,11 +74,24 @@ s8 BNO055_tty_init(int fd,struct bno055_t *bno055)
        return BNO055_SUCCESS;
 }
 
+s8 BNO055_tty_close(struct bno055_t *bno055)
+{
+        if(tcsetattr(bno055->dev_addr,TCSANOW,&old_tty)) return BNO055_ERROR;
+
+       bno055->bus_write=NULL;
+       bno055->bus_read=NULL;
+       bno055->delay_msec=NULL;
+       bno055->dev_addr=0;
+
+       return BNO055_SUCCESS;
+}
+
 s8 BNO055_tty_write(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
 {
        u8 array[BUFFER_LEN];
        u8 i=0;
        int n=0;
+       int c=0;
 
        tcflush(dev_addr,TCIOFLUSH);
        array[0]=BNO055_UART_START;
@@ -80,14 +100,14 @@ s8 BNO055_tty_write(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
        array[3]=cnt;
        for (i=0;i < cnt;i++) array[i+4]=*(reg_data+i);
        
-       n=0;
-       while(n < cnt+4) if((n+=write(dev_addr,array+n,cnt+4-n))==-1) return BNO055_ERROR;
+       for(n=0;n < cnt+4;n+=c)
+               if((c=write(dev_addr,array+n,cnt+4-n))==-1) return BNO055_ERROR;
+
+       for(n=0;n < 2; n+=c)
+               if((c=read(dev_addr,array+n,2-n)) <= 0) return BNO055_ERROR;
 
-       // Add a timeout? KISS approach for now...
-       n=0;
-       while(n < 2) if((n+=read(dev_addr,array+n,2-n))==-1) return BNO055_ERROR;
        if(array[0] != BNO055_UART_ACK || array[1] != BNO055_UART_SUCCESS) return BNO055_ERROR;
-       
+
        return BNO055_SUCCESS;
 }
 
@@ -96,6 +116,7 @@ s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
        u8 array[BUFFER_LEN];
        int n=0;
        int length=0;
+       int c=0;
 
        tcflush(dev_addr,TCIOFLUSH);
        array[0]=BNO055_UART_START;
@@ -103,20 +124,19 @@ s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
        array[2]=reg_addr;
        array[3]=cnt;
        
-       n=0;
-       while(n < 4) if((n+=write(dev_addr,array+n,4-n))==-1) return BNO055_ERROR;
+       for(n=0;n < 4;n+=c)
+               if((c=write(dev_addr,array+n,4-n))==-1) return BNO055_ERROR;
 
-       // Add a timeout? KISS approach for now...
-       n=0;
-       while(n < 2) if((n+=read(dev_addr,array+n,2-n))==-1) return BNO055_ERROR;
+       for(n=0;n < 2; n+=c)
+               if((c=read(dev_addr,array+n,2-n)) <= 0) return BNO055_ERROR;
        
        switch(array[0])
        {
                case BNO055_UART_REPLY:
                        length=array[1];
                        if(length > BUFFER_LEN) length=BUFFER_LEN;
-                       n=0;
-                       while(n < length && n < cnt) if((n+=read(dev_addr,reg_data+n,length-n))==-1) return BNO055_ERROR;
+                       for(n=0;n < length && n < cnt;n+=c)
+                               if((c=read(dev_addr,reg_data+n,length-n)) <= 0) return BNO055_ERROR;
                        break;
                        
                case BNO055_UART_ACK:

diff --git a/test/bno055_test.c b/test/bno055_test.c
index db9c382..f353a0f 100644 (file)
--- a/test/bno055_test.c
+++ b/test/bno055_test.c
@@ -1,3 +1,23 @@
+/******************************************************************************
+                             BNO055 Test
+          Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        General Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
 #include <errno.h>
 #include <fcntl.h>
 #include <stdio.h>
@@ -160,6 +180,7 @@ int main(int argc,char *argv[])
                printf("Temperature=%G degrees Celsius\n",d_temp);
        else fprintf(stderr,"Error while reading temperature in degrees Celsius.\n");
 
+       BNO055_tty_close(&bno055);
        close(fd);
        return 0;
 }