altos: Init BMX160 more carefully

Check accel and gyro power status after turning them on, waiting
for a while to see if they actually power up.

Read more registers after configuring accel to try and get things
reset better.

Signed-off-by: Keith Packard <keithp@keithp.com>
Oops.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c
index e4397fc4a42f55351b2f05451c0a9249165121ea..f9584e14d2f9543dcc79d4d9d39553d145f6d072 100644 (file)
--- 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:

diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h
index e62f172da9bfe8711f23a32b26a96b20c8c91b0a..5552b536ffa385ec86a93c58feabe017d4e8fc34 100644 (file)
--- a/src/drivers/ao_bmx160.h
+++ b/src/drivers/ao_bmx160.h
@@ -61,10 +61,12 @@ ao_bmx160_init(void);
 #define   BMX160_PMU_STATUS_GYR_PMU_STATUS_SUSPEND             0
 #define   BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL              1
 #define   BMX160_PMU_STATUS_GYR_PMU_STATUS_FAST_START_UP       3
+#define   BMX160_PMU_STATUS_GYR_PMU_STATUS_MASK                        3
 #define  BMX160_PMU_STATUS_ACC_PMU_STATUS      4
 #define   BMX160_PMU_STATUS_ACC_PMU_STATUS_SUSPEND             0
 #define   BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL              1
 #define   BMX160_PMU_STATUS_ACC_PMU_STATUS_LOW_POWER           2
+#define   BMX160_PMU_STATUS_ACC_PMU_STATUS_MASK                        3
 #define BMX160_DATA_0                  0x04
 #define BMX160_MAG_X_0_7               0x04
 #define BMX160_MAG_X_8_15              0x05