Fix write.

diff --git a/lib/bno055_tty.c b/lib/bno055_tty.c
index d27e1a7..2e23cc9 100644 (file)
--- a/lib/bno055_tty.c
+++ b/lib/bno055_tty.c
@@ -71,13 +71,13 @@ 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);
        
-       if(write(dev_addr,array,cnt+4)==-1) return BNO055_ERROR;
+       n=0;
+       while(n < cnt+4) if((n+=write(dev_addr,array+n,cnt+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;
-       if(array[0] != BNO055_UART_ACK ||
-               array[1] != BNO055_UART_SUCCESS) return BNO055_ERROR;
+       if(array[0] != BNO055_UART_ACK || array[1] != BNO055_UART_SUCCESS) return BNO055_ERROR;
        
        return BNO055_SUCCESS;
 }
@@ -85,7 +85,6 @@ s8 BNO055_tty_write(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
 s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
 {
        u8 array[BUFFER_LEN];
-       u8 i=0;
        int n=0;
        int length=0;
 
@@ -95,7 +94,8 @@ s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
        array[2]=reg_addr;
        array[3]=cnt;
        
-       if(write(dev_addr,array,4)==-1) return BNO055_ERROR;
+       n=0;
+       while(n < 4) if((n+=write(dev_addr,array+n,4-n))==-1) return BNO055_ERROR;
 
        // Add a timeout? KISS approach for now...
        n=0;
@@ -107,8 +107,7 @@ s8 BNO055_tty_read(u8 dev_addr,u8 reg_addr,u8 *reg_data,u8 cnt)
                        length=array[1];
                        if(length > BUFFER_LEN) length=BUFFER_LEN;
                        n=0;
-                       while(n < length) if((n+=read(dev_addr,array+n,length-n))==-1) return BNO055_ERROR;
-                       for(i=0;i < cnt && i < length;i++) *(reg_data+i)=array[i];
+                       while(n < length && n < cnt) if((n+=read(dev_addr,reg_data+n,length-n))==-1) return BNO055_ERROR;
                        break;
                        
                case BNO055_UART_ACK:

diff --git a/test/bno055_test.c b/test/bno055_test.c
index 2820eef..f61652c 100644 (file)
--- a/test/bno055_test.c
+++ b/test/bno055_test.c
@@ -57,7 +57,8 @@ int main(int argc,char *argv[])
                                        perror(argv[0]);
                                        return -errno;
                                }
-                               BNO055_tty_init(fd,&bno055);
+                               if(BNO055_tty_init(fd,&bno055) != BNO055_SUCCESS)
+                                       fprintf(stderr,"Error while initializing tty interface.\n");
                                break;
                        
                        default:
@@ -68,8 +69,7 @@ int main(int argc,char *argv[])
        if(bno055_init(&bno055) == BNO055_SUCCESS)
        {
                printf("Chip id=0x%02x\n",bno055.chip_id);
-               printf("Software revision id=0x%04x\n",bno055.sw_rev_id);
-               printf("Page id=0x%02x\n",bno055.page_id);
+               printf("Software revision id=%d.%d.%d.%d\n",bno055.sw_rev_id >> 12,(bno055.sw_rev_id >> 8)  & 0x0f,(bno055.sw_rev_id >> 4) & 0x0f,bno055.sw_rev_id & 0x0f);
                printf("Accelerometer id=0x%02x\n",bno055.accel_rev_id);
                printf("Magnetometer id=0x%02x\n",bno055.mag_rev_id);
                printf("Gyroscope id=0x%02x\n",bno055.gyro_rev_id);
@@ -77,14 +77,13 @@ int main(int argc,char *argv[])
        }
        else fprintf(stderr,"Error while initializing bno055.\n");
 
-               
        if(bno055_set_power_mode(BNO055_POWER_MODE_NORMAL) != BNO055_SUCCESS)
                fprintf(stderr,"Error while setting power mode.\n");
 
-       /* Raw data */
+       printf("Raw data:\n");
        if(bno055_set_operation_mode(BNO055_OPERATION_MODE_AMG) != BNO055_SUCCESS)
                fprintf(stderr,"Error while setting operation mode.\n");
-               
+
        if(bno055_read_accel_xyz(&accel) == BNO055_SUCCESS)
                printf("Acceleration=%d, %d, %d\n",accel.x,accel.y,accel.z);
        else fprintf(stderr,"Error while reading acceleration.\n");
@@ -104,7 +103,7 @@ int main(int argc,char *argv[])
        if(bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF) != BNO055_SUCCESS)
                fprintf(stderr,"Error while setting operation mode.\n");
 
-       /* Fused data */
+       printf("Fused data:\n");
        if(bno055_read_euler_hrp(&euler) == BNO055_SUCCESS)
                printf("Euler angles=%d, %d, %d\n",euler.h,euler.r,euler.p);
        else fprintf(stderr,"Error while reading Euler angles.\n");
@@ -118,7 +117,7 @@ int main(int argc,char *argv[])
        else fprintf(stderr,"Error while reading linear acceleration.\n");
 
        if(bno055_read_gravity_xyz(&gravity) == BNO055_SUCCESS)
-               printf("Gravity=%d, %d, %d\n",linear_accel.x,linear_accel.y,linear_accel.z);
+               printf("Gravity=%d, %d, %d\n",gravity.x,gravity.y,gravity.z);
        else fprintf(stderr,"Error while reading gravity.\n");
 
        if(bno055_convert_double_accel_xyz_msq(&d_accel) == BNO055_SUCCESS)