From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sat, 2 Feb 2019 06:53:45 +0000 (-0200)
Subject: Add timeout.
X-Git-Tag: f1~6
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=2675427bebb771a64d2ac72cdaf8961ee183616b;p=bno055.git

Add timeout.
---

diff --git a/include/bno055_tty.h b/include/bno055_tty.h
index 117f87c..61e8688 100644
--- 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
--- 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
--- 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;
 }