From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 5 Feb 2019 13:52:38 +0000 (-0200)
Subject: Add repeat write if overrun.
X-Git-Tag: f1~1^2
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c4f23548fc2dbdd40689503de28586bcfd407a02;p=bno055.git

Add repeat write if overrun.
---

diff --git a/lib/bno055_tty.c b/lib/bno055_tty.c
index 8e08f71..6edbb76 100644
--- a/lib/bno055_tty.c
+++ b/lib/bno055_tty.c
@@ -37,6 +37,7 @@
 #define BNO055_UART_REPLY	0xbb
 
 #define BNO055_UART_SUCCESS	0x01
+#define BNO055_UART_OVERRUN	0X07
 
 /* This makes the code non-reentrant,
 but the Bosch BNO055_driver is already non-reentrant */
@@ -100,19 +101,23 @@ s8 BNO055_tty_write(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
 	int n=0;
 	int c=0;
 
-	tcflush(dev_addr,TCIOFLUSH);
-	array[0]=BNO055_UART_START;
-	array[1]=BNO055_UART_WRITE_CMD;
-	array[2]=reg_addr;
-	array[3]=cnt;
-	for (i=0;i < cnt;i++) array[i+4]=*(reg_data+i);
+	do
+	{
+		tcflush(dev_addr,TCIOFLUSH);
+		array[0]=BNO055_UART_START;
+		array[1]=BNO055_UART_WRITE_CMD;
+		array[2]=reg_addr;
+		array[3]=cnt;
+		for (i=0;i < cnt;i++) array[i+4]=*(reg_data+i);
 	
-	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 < 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;
+		for(n=0;n < 2; n+=c)
+			if((c=read(dev_addr,array+n,2-n)) <= 0) return BNO055_ERROR;
 
+	} while(array[0] == BNO055_UART_ACK && array[1] == BNO055_UART_OVERRUN);
+	
 	if(array[0] != BNO055_UART_ACK || array[1] != BNO055_UART_SUCCESS) return BNO055_ERROR;
 
 	return BNO055_SUCCESS;
@@ -125,17 +130,20 @@ s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
 	int length=0;
 	int c=0;
 
-	tcflush(dev_addr,TCIOFLUSH);
-	array[0]=BNO055_UART_START;
-	array[1]=BNO055_UART_READ_CMD;
-	array[2]=reg_addr;
-	array[3]=cnt;
+	do
+	{
+		tcflush(dev_addr,TCIOFLUSH);
+		array[0]=BNO055_UART_START;
+		array[1]=BNO055_UART_READ_CMD;
+		array[2]=reg_addr;
+		array[3]=cnt;
 	
-	for(n=0;n < 4;n+=c)
-		if((c=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;
 
-	for(n=0;n < 2; n+=c)
-		if((c=read(dev_addr,array+n,2-n)) <= 0) return BNO055_ERROR;
+		for(n=0;n < 2; n+=c)
+			if((c=read(dev_addr,array+n,2-n)) <= 0) return BNO055_ERROR;
+	} while(array[0] == BNO055_UART_ACK && array[1] == BNO055_UART_OVERRUN);
 	
 	switch(array[0])
 	{