From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 31 Jan 2019 05:30:24 +0000 (-0200)
Subject: Fix write.
X-Git-Tag: f1~11
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=02f0eed87e107a9fc623e10b6aeb69d9a06c5d55;p=bno055.git

Fix write.
---

diff --git a/lib/bno055_tty.c b/lib/bno055_tty.c
index d27e1a7..2e23cc9 100644
--- 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
--- 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)