X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu9250.c;h=9dce9f1a5a0d4f79f241c50e2a238a09d2b2e906;hp=ae8dacd0a2eee8544e6de5e8061c316ffaacf17b;hb=c417ab1de2a083b5fcff2e081e4feb2a65887903;hpb=c6e57291d91f1f6c4de5c54a5cfd3eef66d9f830

diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c
index ae8dacd0..9dce9f1a 100644
--- a/src/drivers/ao_mpu9250.c
+++ b/src/drivers/ao_mpu9250.c
@@ -472,10 +472,9 @@ ao_mpu9250_read(void)
 	uint8_t	addr;
 	uint8_t val;
 
-	ao_cmd_hex();
+	addr = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	addr = ao_cmd_lex_i;
 	ao_mpu9250_spi_get();
 	val = _ao_mpu9250_reg_read(addr);
 	ao_mpu9250_spi_put();
@@ -488,14 +487,12 @@ ao_mpu9250_write(void)
 	uint8_t	addr;
 	uint8_t val;
 
-	ao_cmd_hex();
+	addr = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	addr = ao_cmd_lex_i;
-	ao_cmd_hex();
+	val = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	val = ao_cmd_lex_i;
 	printf("Addr %02x val %02x\n", addr, val);
 	ao_mpu9250_spi_get();
 	_ao_mpu9250_reg_write(addr, val);
@@ -508,10 +505,9 @@ ao_mpu9250_mag_read(void)
 	uint8_t	addr;
 	uint8_t val;
 
-	ao_cmd_hex();
+	addr = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	addr = ao_cmd_lex_i;
 	ao_mpu9250_spi_get();
 	val = _ao_mpu9250_mag_reg_read(addr);
 	ao_mpu9250_spi_put();
@@ -524,14 +520,12 @@ ao_mpu9250_mag_write(void)
 	uint8_t	addr;
 	uint8_t val;
 
-	ao_cmd_hex();
+	addr = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	addr = ao_cmd_lex_i;
-	ao_cmd_hex();
+	val = ao_cmd_hex();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	val = ao_cmd_lex_i;
 	printf("Addr %02x val %02x\n", addr, val);
 	ao_mpu9250_spi_get();
 	_ao_mpu9250_mag_reg_write(addr, val);