From 0571531066918fdefe9447f3b4192d0c6c477afa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 10:48:24 -0700 Subject: [PATCH] altos: Grab SPI mutex until MPU6000 I2C mode is disabled If other drivers use the SPI bus, the MPU6000 gets confused as its sitting on the bus looking for I2C messages. Just grab the mutex before the OS is running and hold onto it until the MPU6000 has been initialized. Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.c | 265 +++++++++++++++++++++++---------------- src/drivers/ao_mpu6000.h | 14 +-- src/stm/ao_arch_funcs.h | 7 +- 3 files changed, 168 insertions(+), 118 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 diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index f01e9e83..a42f69c4 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -133,13 +133,13 @@ #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET 1 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET 0 -#define MPU6000_USER_CONTROL 0x6a -#define MPU6000_USER_CONTROL_FIFO_EN 6 -#define MPU6000_USER_CONTROL_I2C_MST_EN 5 -#define MPU6000_USER_CONTROL_I2C_IF_DIS 4 -#define MPU6000_USER_CONTROL_FIFO_RESET 2 -#define MPU6000_USER_CONTROL_I2C_MST_RESET 1 -#define MPU6000_USER_CONTROL_SIG_COND_RESET 0 +#define MPU6000_USER_CTRL 0x6a +#define MPU6000_USER_CTRL_FIFO_EN 6 +#define MPU6000_USER_CTRL_I2C_MST_EN 5 +#define MPU6000_USER_CTRL_I2C_IF_DIS 4 +#define MPU6000_USER_CTRL_FIFO_RESET 2 +#define MPU6000_USER_CTRL_I2C_MST_RESET 1 +#define MPU6000_USER_CTRL_SIG_COND_RESET 0 #define MPU6000_PWR_MGMT_1 0x6b #define MPU6000_PWR_MGMT_1_DEVICE_RESET 7 diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index 1e78cabc..6fe86e62 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -87,13 +87,16 @@ extern uint16_t ao_spi_speed[STM_NUM_SPI]; void ao_spi_init(void); +#define ao_spi_set_cs(reg,mask) ((reg)->bsrr = ((uint32_t) (mask)) << 16) +#define ao_spi_clr_cs(reg,mask) ((reg)->bsrr = (mask)) + #define ao_spi_get_mask(reg,mask,bus, speed) do { \ ao_spi_get(bus, speed); \ - (reg)->bsrr = ((uint32_t) mask) << 16; \ + ao_spi_set_cs(reg,mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ - (reg)->bsrr = mask; \ + ao_spi_clr_cs(reg,mask); \ ao_spi_put(bus); \ } while (0) -- 2.30.2