altos: Fix command-line FAT filename parsing

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

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index a1c32d4d836db80808389009a8b38d0e6d2565ee..6d47482cc0c31bac633cf66014e5d9dd57bdf16e 100644 (file)
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -199,6 +199,24 @@ ao_mpu6000_setup(void)
        ao_delay(AO_MS_TO_TICKS(200));
        ao_mpu6000_sample(&test_mode);
 
+#if TRIDGE
+       // read the product ID rev c has 1/2 the sensitivity of rev d
+    _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+    //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
+
+    if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
+        (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
+        // Accel scale 8g (4096 LSB/g)
+        // Rev C has different scaling than rev D
+        register_write(MPUREG_ACCEL_CONFIG,1<<3);
+    } else {
+        // Accel scale 8g (4096 LSB/g)
+        register_write(MPUREG_ACCEL_CONFIG,2<<3);
+    }
+    hal.scheduler->delay(1);
+
+#endif
+
        /* Configure accelerometer to +/-16G */
        ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
                             (0 << MPU600_ACCEL_CONFIG_XA_ST) |
@@ -225,7 +243,7 @@ ao_mpu6000_setup(void)
        errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
 
        if (errors)
-               ao_panic(AO_PANIC_SELF_TEST);
+               ao_panic(AO_PANIC_SELF_TEST_MPU6000);
 
        /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
        ao_mpu6000_reg_write(MPU6000_CONFIG,
@@ -240,8 +258,7 @@ ao_mpu6000_setup(void)
        ao_mpu6000_configured = 1;
 }
 
-struct ao_mpu6000_sample ao_mpu6000_current;
-uint8_t ao_mpu6000_valid;
+struct ao_mpu6000_sample       ao_mpu6000_current;
 
 static void
 ao_mpu6000(void)
@@ -249,13 +266,11 @@ ao_mpu6000(void)
        ao_mpu6000_setup();
        for (;;)
        {
-               struct ao_mpu6000_sample ao_mpu6000_next;
-               ao_mpu6000_sample(&ao_mpu6000_next);
+               ao_mpu6000_sample(&ao_mpu6000_current);
                ao_arch_critical(
-                       ao_mpu6000_current = ao_mpu6000_next;
-                       ao_mpu6000_valid = 1;
+                       AO_DATA_PRESENT(AO_DATA_MPU6000);
+                       AO_DATA_WAIT();
                        );
-               ao_delay(0);
        }
 }
 
@@ -264,18 +279,16 @@ static struct ao_task ao_mpu6000_task;
 static void
 ao_mpu6000_show(void)
 {
-       struct ao_mpu6000_sample        sample;
+       struct ao_data  sample;
 
-       ao_arch_critical(
-               sample = ao_mpu6000_current;
-               );
+       ao_data_get(&sample);
        printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
-               sample.accel_x,
-               sample.accel_y,
-               sample.accel_z,
-               sample.gyro_x,
-               sample.gyro_y,
-               sample.gyro_z);
+               sample.mpu6000.accel_x,
+               sample.mpu6000.accel_y,
+               sample.mpu6000.accel_z,
+               sample.mpu6000.gyro_x,
+               sample.mpu6000.gyro_y,
+               sample.mpu6000.gyro_z);
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
@@ -287,7 +300,6 @@ void
 ao_mpu6000_init(void)
 {
        ao_mpu6000_configured = 0;
-       ao_mpu6000_valid = 0;
 
        ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
        ao_cmd_register(&ao_mpu6000_cmds[0]);