X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_bmx160.c;fp=src%2Fdrivers%2Fao_bmx160.c;h=f9584e14d2f9543dcc79d4d9d39553d145f6d072;hp=e4397fc4a42f55351b2f05451c0a9249165121ea;hb=88cb0f4a791e0867ccd279e6d244d94f0b1765e9;hpb=0ca9a7c6fc914c4b555c99863828861ec54fa01d

diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c
index e4397fc4..f9584e14 100644
--- a/src/drivers/ao_bmx160.c
+++ b/src/drivers/ao_bmx160.c
@@ -198,8 +198,37 @@ _ao_bmx160_setup(void)
 	 */
 
 	_ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL));
+
+	for (r = 0; r < 20; r++) {
+		ao_delay(AO_MS_TO_TICKS(100));
+		if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS)
+		      >> BMX160_PMU_STATUS_ACC_PMU_STATUS)
+		     & BMX160_PMU_STATUS_ACC_PMU_STATUS_MASK)
+		    == BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL)
+		{
+			r = 0;
+			break;
+		}
+	}
+	if (r != 0)
+		AO_SENSOR_ERROR(AO_DATA_BMX160);
+
 	_ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL));
 
+	for (r = 0; r < 20; r++) {
+		ao_delay(AO_MS_TO_TICKS(100));
+		if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS)
+		      >> BMX160_PMU_STATUS_GYR_PMU_STATUS)
+		     & BMX160_PMU_STATUS_GYR_PMU_STATUS_MASK)
+		    == BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL)
+		{
+			r = 0;
+			break;
+		}
+	}
+	if (r != 0)
+		AO_SENSOR_ERROR(AO_DATA_BMX160);
+
 	/* Configure accelerometer:
 	 *
 	 * 	undersampling disabled
@@ -215,7 +244,8 @@ _ao_bmx160_setup(void)
 			     (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR));
 	_ao_bmx160_reg_write(BMX160_ACC_RANGE,
 			     BMX160_ACC_RANGE_16G);
-	for (r = 0x4; r <= 0x17; r++)
+
+	for (r = 0x3; r <= 0x1b; r++)
 		(void) _ao_bmx160_reg_read(r);
 
 	/* Configure gyro: