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) |
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,
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();