Add repeat write if overrun.

diff --git a/lib/bno055_tty.c b/lib/bno055_tty.c
index 8e08f71..6edbb76 100644 (file)
--- 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])
        {