altos: Init BMX160 more carefully

[fw/altos] / src / drivers / ao_bmx160.c

diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c
index 4c3c2a9f935cce8628924db4b17999cda2febd95..f9584e14d2f9543dcc79d4d9d39553d145f6d072 100644 (file)
--- a/src/drivers/ao_bmx160.c
+++ b/src/drivers/ao_bmx160.c
@@ -90,6 +90,7 @@ _ao_bmm150_reg_write(uint8_t addr, uint8_t data)
        _ao_bmm150_wait_manual();
 }
 
+#if BMX160_TEST
 static uint8_t
 _ao_bmm150_reg_read(uint8_t addr)
 {
@@ -97,6 +98,7 @@ _ao_bmm150_reg_read(uint8_t addr)
        _ao_bmm150_wait_manual();
        return _ao_bmx160_reg_read(BMX160_DATA_0);
 }
+#endif
 
 static void
 _ao_bmx160_sample(struct ao_bmx160_sample *sample)
@@ -181,18 +183,52 @@ _ao_bmx160_wait_alive(void)
 static void
 _ao_bmx160_setup(void)
 {
+       int r;
+
        if (ao_bmx160_configured)
                return;
 
        /* Make sure the chip is responding */
        _ao_bmx160_wait_alive();
 
-       /* Reboot */
-       _ao_bmx160_cmd(BMX160_CMD_SOFTRESET);
-
        /* Force SPI mode */
        _ao_bmx160_reg_write(BMX160_NV_CONF, 1 << BMX160_NV_CONF_SPI_EN);
 
+       /* Enable acc and gyr
+        */
+
+       _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
@@ -209,6 +245,9 @@ _ao_bmx160_setup(void)
        _ao_bmx160_reg_write(BMX160_ACC_RANGE,
                             BMX160_ACC_RANGE_16G);
 
+       for (r = 0x3; r <= 0x1b; r++)
+               (void) _ao_bmx160_reg_read(r);
+
        /* Configure gyro:
         *
         *      200Hz sampling rate
@@ -274,11 +313,6 @@ _ao_bmx160_setup(void)
                             (0 << BMX160_MAG_IF_0_MAG_OFFSET) |
                             (0 << BMX160_MAG_IF_0_MAG_RD_BURST));
 
-       /* Enable acc and gyr
-        */
-
-       _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL));
-       _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL));
        ao_bmx160_configured = 1;
 }