static uint8_t ao_mpu6000_wake;
static uint8_t ao_mpu6000_configured;
-static void
-ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
-{
- ao_i2c_get(AO_MPU6000_I2C_INDEX);
- ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
- ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
- ao_i2c_send(data, len, AO_MPU6000_I2C_INDEX, TRUE);
- ao_i2c_put(AO_MPU6000_I2C_INDEX);
-}
+#define ao_mpu6000_spi_get() ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT, \
+ AO_MPU6000_SPI_CS_PIN, \
+ AO_MPU6000_SPI_CS, \
+ AO_MPU6000_SPI_BUS, \
+ AO_SPI_SPEED_1MHz)
+
+#define ao_mpu6000_spi_put() ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT, \
+ AO_MPU6000_SPI_CS_PIN, \
+ AO_MPU6000_SPI_CS, \
+ AO_MPU6000_SPI_BUS)
+
static void
ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
{
uint8_t d[2] = { addr, value };
+#ifdef AO_MPU6000_I2C_INDEX
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+ ao_mpu6000_spi_get();
+ ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_put();
+#endif
}
static void
ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
{
+#ifdef AO_MPU6000_I2C_INDEX
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+ addr |= 0x80;
+ ao_mpu6000_spi_get();
+ ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+ ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_put();
+#endif
}
static uint8_t
ao_mpu6000_reg_read(uint8_t addr)
{
uint8_t value;
+#ifdef AO_MPU6000_I2C_INDEX
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+ addr |= 0x80;
+ ao_mpu6000_spi_get();
+ ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+ ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_put();
+#endif
return value;
}
int i = sizeof (*sample) / 2;
ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
- /* byte swap (sigh) */
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+ /* byte swap */
while (i--) {
uint16_t t = *d;
*d++ = (t >> 8) | (t << 8);
}
+#endif
}
#define G 981 /* in cm/s² */
int16_t diff = test - normal;
if (diff < MPU6000_ST_ACCEL(16) / 2) {
- printf ("%s accel self test value too small (normal %d, test %d)\n",
- which, normal, test);
- return FALSE;
+ return 1;
}
if (diff > MPU6000_ST_ACCEL(16) * 2) {
- printf ("%s accel self test value too large (normal %d, test %d)\n",
- which, normal, test);
- return FALSE;
+ return 1;
}
- return TRUE;
+ return 0;
}
static uint8_t
if (diff < 0)
diff = -diff;
if (diff < MPU6000_ST_GYRO(2000) / 2) {
- printf ("%s gyro self test value too small (normal %d, test %d)\n",
- which, normal, test);
- return FALSE;
+ return 1;
}
if (diff > MPU6000_ST_GYRO(2000) * 2) {
- printf ("%s gyro self test value too large (normal %d, test %d)\n",
- which, normal, test);
- return FALSE;
+ return 1;
}
- return TRUE;
+ return 0;
}
static void
ao_mpu6000_setup(void)
{
struct ao_mpu6000_sample normal_mode, test_mode;
- int t;
+ int errors =0;
if (ao_mpu6000_configured)
return;
ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
(1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
- while (ao_mpu6000_reg_read(MPU6000_PWR_MGMT_1) &
- (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET))
- ao_yield();
+
+ /* Wait for it to reset. If we talk too quickly, it appears to get confused */
+ ao_delay(AO_MS_TO_TICKS(100));
/* Reset signal conditioning */
ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
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) |
ao_delay(AO_MS_TO_TICKS(10));
ao_mpu6000_sample(&normal_mode);
- ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
- ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
- ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
- ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
- ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
- ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+
+ if (errors)
+ 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(&ao_mpu6000_current);
+ ao_arch_critical(
+ AO_DATA_PRESENT(AO_DATA_MPU6000);
+ AO_DATA_WAIT();
+ );
+ }
+}
+
+static struct ao_task ao_mpu6000_task;
static void
ao_mpu6000_show(void)
{
- struct ao_mpu6000_sample sample;
+ struct ao_data sample;
- ao_mpu6000_setup();
- ao_mpu6000_sample(&sample);
+ ao_data_get(&sample);
printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
- ao_mpu6000_accel(sample.accel_x),
- ao_mpu6000_accel(sample.accel_y),
- ao_mpu6000_accel(sample.accel_z),
- ao_mpu6000_gyro(sample.gyro_x),
- ao_mpu6000_gyro(sample.gyro_y),
- ao_mpu6000_gyro(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[] = {
{
ao_mpu6000_configured = 0;
+ ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
ao_cmd_register(&ao_mpu6000_cmds[0]);
}
projects / fw / altos / blobdiff