diff options
Diffstat (limited to 'src/drivers/ao_mpu6000.c')
| -rw-r--r-- | src/drivers/ao_mpu6000.c | 265 |
1 files changed, 156 insertions, 109 deletions
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index fc768cc9..73057820 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -24,81 +24,89 @@ static uint8_t ao_mpu6000_wake; static uint8_t ao_mpu6000_configured; -#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) +#ifndef AO_MPU6000_I2C_INDEX +#define AO_MPU6000_SPI 1 +#else +#define AO_MPU6000_SPI 0 +#endif + +#if AO_MPU6000_SPI -#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) +#define ao_mpu6000_spi_get() ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz) +#define ao_mpu6000_spi_put() ao_spi_put(AO_MPU6000_SPI_BUS) + +#define ao_mpu6000_spi_start() ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT, \ + (1 << AO_MPU6000_SPI_CS_PIN)) + +#define ao_mpu6000_spi_end() ao_spi_clr_cs(AO_MPU6000_SPI_CS_PORT, \ + (1 << AO_MPU6000_SPI_CS_PIN)) + +#endif static void -ao_mpu6000_reg_write(uint8_t addr, uint8_t value) +_ao_mpu6000_reg_write(uint8_t addr, uint8_t value) { uint8_t d[2] = { addr, value }; -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + ao_mpu6000_spi_start(); + ao_spi_send(d, 2, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else 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) +_ao_mpu6000_read(uint8_t addr, void *data, uint8_t len) { -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + addr |= 0x80; + ao_mpu6000_spi_start(); + ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); + ao_spi_recv(data, len, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else 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) +_ao_mpu6000_reg_read(uint8_t addr) { uint8_t value; -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + addr |= 0x80; + ao_mpu6000_spi_start(); + ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); + ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else 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; } static void -ao_mpu6000_sample(struct ao_mpu6000_sample *sample) +_ao_mpu6000_sample(struct ao_mpu6000_sample *sample) { uint16_t *d = (uint16_t *) sample; int i = sizeof (*sample) / 2; - ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample)); + _ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample)); #if __BYTE_ORDER == __LITTLE_ENDIAN /* byte swap */ while (i--) { @@ -153,7 +161,22 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which) } static void -ao_mpu6000_setup(void) +_ao_mpu6000_wait_alive(void) +{ + uint8_t i; + + /* Wait for the chip to wake up */ + for (i = 0; i < 30; i++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68) + break; + } + if (i == 30) + ao_panic(AO_PANIC_SELF_TEST_MPU6000); +} + +static void +_ao_mpu6000_setup(void) { struct ao_mpu6000_sample normal_mode, test_mode; int errors =0; @@ -161,104 +184,107 @@ ao_mpu6000_setup(void) if (ao_mpu6000_configured) return; + _ao_mpu6000_wait_alive(); + /* Reset the whole chip */ - ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, - (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET)); + _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, + (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET)); /* 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, - (0 << MPU6000_USER_CONTROL_FIFO_EN) | - (0 << MPU6000_USER_CONTROL_I2C_MST_EN) | - (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) | - (0 << MPU6000_USER_CONTROL_FIFO_RESET) | - (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) | - (1 << MPU6000_USER_CONTROL_SIG_COND_RESET)); + _ao_mpu6000_wait_alive(); + + /* Reset signal conditioning, disabling I2C on SPI systems */ + _ao_mpu6000_reg_write(MPU6000_USER_CTRL, + (0 << MPU6000_USER_CTRL_FIFO_EN) | + (0 << MPU6000_USER_CTRL_I2C_MST_EN) | + (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) | + (0 << MPU6000_USER_CTRL_FIFO_RESET) | + (0 << MPU6000_USER_CTRL_I2C_MST_RESET) | + (1 << MPU6000_USER_CTRL_SIG_COND_RESET)); - while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET)) - ao_yield(); + while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET)) + ao_delay(AO_MS_TO_TICKS(10)); /* Reset signal paths */ - ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, - (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | - (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | - (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); + _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, + (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | + (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | + (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); - ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, - (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | - (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | - (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); + _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, + (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | + (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | + (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); /* Select clocks, disable sleep */ - ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, - (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) | - (0 << MPU6000_PWR_MGMT_1_SLEEP) | - (0 << MPU6000_PWR_MGMT_1_CYCLE) | - (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) | - (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL)); + _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, + (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) | + (0 << MPU6000_PWR_MGMT_1_SLEEP) | + (0 << MPU6000_PWR_MGMT_1_CYCLE) | + (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) | + (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL)); - /* Set sample rate divider to sample at full speed - ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0); + /* Set sample rate divider to sample at full speed */ + _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0); /* Disable filtering */ - ao_mpu6000_reg_write(MPU6000_CONFIG, - (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | - (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG)); + _ao_mpu6000_reg_write(MPU6000_CONFIG, + (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | + (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG)); /* Configure accelerometer to +/-16G in self-test mode */ - ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, - (1 << MPU600_ACCEL_CONFIG_XA_ST) | - (1 << MPU600_ACCEL_CONFIG_YA_ST) | - (1 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (1 << MPU600_ACCEL_CONFIG_XA_ST) | + (1 << MPU600_ACCEL_CONFIG_YA_ST) | + (1 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); /* Configure gyro to +/- 2000°/s in self-test mode */ - ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (1 << MPU600_GYRO_CONFIG_XG_ST) | - (1 << MPU600_GYRO_CONFIG_YG_ST) | - (1 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (1 << MPU600_GYRO_CONFIG_XG_ST) | + (1 << MPU600_GYRO_CONFIG_YG_ST) | + (1 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); ao_delay(AO_MS_TO_TICKS(200)); - ao_mpu6000_sample(&test_mode); + _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); + _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) | - (0 << MPU600_ACCEL_CONFIG_YA_ST) | - (0 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (0 << MPU600_ACCEL_CONFIG_XA_ST) | + (0 << MPU600_ACCEL_CONFIG_YA_ST) | + (0 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); /* Configure gyro to +/- 2000°/s */ - ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (0 << MPU600_GYRO_CONFIG_XG_ST) | - (0 << MPU600_GYRO_CONFIG_YG_ST) | - (0 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (0 << MPU600_GYRO_CONFIG_XG_ST) | + (0 << MPU600_GYRO_CONFIG_YG_ST) | + (0 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); ao_delay(AO_MS_TO_TICKS(10)); - ao_mpu6000_sample(&normal_mode); + _ao_mpu6000_sample(&normal_mode); 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"); @@ -272,13 +298,13 @@ ao_mpu6000_setup(void) 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, - (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | - (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG)); + _ao_mpu6000_reg_write(MPU6000_CONFIG, + (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | + (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG)); /* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */ - ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, - 1000 / 200 - 1); + _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, + 1000 / 200 - 1); ao_delay(AO_MS_TO_TICKS(100)); ao_mpu6000_configured = 1; @@ -289,10 +315,20 @@ struct ao_mpu6000_sample ao_mpu6000_current; static void ao_mpu6000(void) { - ao_mpu6000_setup(); + /* ao_mpu6000_init already grabbed the SPI bus and mutex */ + _ao_mpu6000_setup(); +#if AO_MPU6000_SPI + ao_mpu6000_spi_put(); +#endif for (;;) { - ao_mpu6000_sample(&ao_mpu6000_current); +#if AO_MPU6000_SPI + ao_mpu6000_spi_get(); +#endif + _ao_mpu6000_sample(&ao_mpu6000_current); +#if AO_MPU6000_SPI + ao_mpu6000_spi_put(); +#endif ao_arch_critical( AO_DATA_PRESENT(AO_DATA_MPU6000); AO_DATA_WAIT(); @@ -328,9 +364,20 @@ ao_mpu6000_init(void) ao_mpu6000_configured = 0; ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000"); -#ifndef AO_MPU6000_I2C_INDEX + +#if AO_MPU6000_SPI ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN)); + + /* Pretend to be the mpu6000 task. Grab the SPI bus right away and + * hold it for the task so that nothing else uses the SPI bus before + * we get the I2C mode disabled in the chip + */ + + ao_cur_task = &ao_mpu6000_task; + ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz); + ao_cur_task = NULL; #endif + ao_cmd_register(&ao_mpu6000_cmds[0]); } #endif |