X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.c;h=6d47482cc0c31bac633cf66014e5d9dd57bdf16e;hb=182ceaac7d91dc6e9ebac6455d5de0c10687796b;hp=e8c80f12428943bf365977a03dbc9695656003ac;hpb=f89e7de20374141b367205aa517a08ee203bfaf3;p=fw%2Faltos

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index e8c80f12..6d47482c 100644
--- 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,13 +258,15 @@ ao_mpu6000_setup(void)
 	ao_mpu6000_configured = 1;
 }
 
+struct ao_mpu6000_sample	ao_mpu6000_current;
+
 static void
 ao_mpu6000(void)
 {
 	ao_mpu6000_setup();
 	for (;;)
 	{
-		ao_mpu6000_sample((struct ao_mpu6000_sample *) &ao_data_ring[ao_data_head].mpu6000);
+		ao_mpu6000_sample(&ao_mpu6000_current);
 		ao_arch_critical(
 			AO_DATA_PRESENT(AO_DATA_MPU6000);
 			AO_DATA_WAIT();