diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/ao_mpu9250.c | 180 | ||||
| -rw-r--r-- | src/drivers/ao_mpu9250.h | 71 |
2 files changed, 241 insertions, 10 deletions
diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c index b79f27ca..ae8dacd0 100644 --- a/src/drivers/ao_mpu9250.c +++ b/src/drivers/ao_mpu9250.c @@ -22,6 +22,8 @@ #if HAS_MPU9250 +#define MPU9250_TEST 0 + static uint8_t ao_mpu9250_configured; extern uint8_t ao_sensor_errors; @@ -43,8 +45,12 @@ extern uint8_t ao_sensor_errors; #define ao_mpu9250_spi_end() ao_spi_clr_cs(AO_MPU9250_SPI_CS_PORT, \ (1 << AO_MPU9250_SPI_CS_PIN)) -#endif +#else +#define ao_mpu9250_spi_get() +#define ao_mpu9250_spi_put() + +#endif static void _ao_mpu9250_reg_write(uint8_t addr, uint8_t value) @@ -103,6 +109,61 @@ _ao_mpu9250_reg_read(uint8_t addr) } static void +_ao_mpu9250_slv4_setup(uint8_t addr, uint8_t reg) +{ + /* Set i2c slave address */ + _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_ADDR, + addr); + + /* Set i2c register address */ + _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_REG, + reg); +} + +static void +_ao_mpu9250_slv4_run(void) +{ + uint8_t ctrl; + + /* Start the transfer */ + _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_CTRL, + (1 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_EN) | + (0 << MPU9250_I2C_SLV4_CTRL_SLV4_DONE_INT_EN) | + (0 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_REG_DIS) | + (0 << MPU9250_I2C_SLV4_CTRL_I2C_MST_DLY)); + + /* Poll for completion */ + for (;;) { + ctrl = _ao_mpu9250_reg_read(MPU9250_I2C_SLV4_CTRL); + if ((ctrl & (1 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_EN)) == 0) + break; + ao_delay(0); + } +} + +static uint8_t +_ao_mpu9250_mag_reg_read(uint8_t reg) +{ + _ao_mpu9250_slv4_setup((1 << 7) | MPU9250_MAG_ADDR, reg); + + _ao_mpu9250_slv4_run(); + + return _ao_mpu9250_reg_read(MPU9250_I2C_SLV4_DI); +} + +static void +_ao_mpu9250_mag_reg_write(uint8_t reg, uint8_t value) +{ + _ao_mpu9250_slv4_setup((0 << 7) | MPU9250_MAG_ADDR, reg); + + /* Set the data */ + _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_DO, + value); + + _ao_mpu9250_slv4_run(); +} + +static void _ao_mpu9250_sample(struct ao_mpu9250_sample *sample) { uint16_t *d = (uint16_t *) sample; @@ -180,6 +241,7 @@ _ao_mpu9250_wait_alive(void) } #define ST_TRIES 10 +#define MAG_TRIES 10 static void _ao_mpu9250_setup(void) @@ -187,6 +249,7 @@ _ao_mpu9250_setup(void) struct ao_mpu9250_sample normal_mode, test_mode; int errors; int st_tries; + int mag_tries; if (ao_mpu9250_configured) return; @@ -205,7 +268,7 @@ _ao_mpu9250_setup(void) /* Reset signal conditioning, disabling I2C on SPI systems */ _ao_mpu9250_reg_write(MPU9250_USER_CTRL, (0 << MPU9250_USER_CTRL_FIFO_EN) | - (0 << MPU9250_USER_CTRL_I2C_MST_EN) | + (1 << MPU9250_USER_CTRL_I2C_MST_EN) | (AO_MPU9250_SPI << MPU9250_USER_CTRL_I2C_IF_DIS) | (0 << MPU9250_USER_CTRL_FIFO_RESET) | (0 << MPU9250_USER_CTRL_I2C_MST_RESET) | @@ -233,6 +296,14 @@ _ao_mpu9250_setup(void) (0 << MPU9250_PWR_MGMT_1_TEMP_DIS) | (MPU9250_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU9250_PWR_MGMT_1_CLKSEL)); + /* Set I2C clock and options */ + _ao_mpu9250_reg_write(MPU9250_MST_CTRL, + (0 << MPU9250_MST_CTRL_MULT_MST_EN) | + (0 << MPU9250_MST_CTRL_WAIT_FOR_ES) | + (0 << MPU9250_MST_CTRL_SLV_3_FIFO_EN) | + (0 << MPU9250_MST_CTRL_I2C_MST_P_NSR) | + (MPU9250_MST_CTRL_I2C_MST_CLK_400 << MPU9250_MST_CTRL_I2C_MST_CLK)); + /* Set sample rate divider to sample at full speed */ _ao_mpu9250_reg_write(MPU9250_SMPRT_DIV, 0); @@ -292,6 +363,53 @@ _ao_mpu9250_setup(void) if (st_tries == ST_TRIES) ao_sensor_errors = 1; + /* Set up the mag sensor */ + + /* make sure it's alive */ + for (mag_tries = 0; mag_tries < MAG_TRIES; mag_tries++) { + if (_ao_mpu9250_mag_reg_read(MPU9250_MAG_WIA) == MPU9250_MAG_WIA_VALUE) + break; + } + + if (mag_tries == MAG_TRIES) + ao_sensor_errors = 1; + + /* Select continuous mode 2 (100Hz), 16 bit samples */ + + _ao_mpu9250_mag_reg_write(MPU9250_MAG_CNTL1, + (MPU9250_MAG_CNTL1_BIT_16 << MPU9250_MAG_CNTL1_BIT) | + (MPU9250_MAG_CNTL1_MODE_CONT_2 << MPU9250_MAG_CNTL1_MODE)); + + /* Set i2c master to delay shadowing data until read is + * complete (avoids tearing the data) */ + + _ao_mpu9250_reg_write(MPU9250_I2C_MST_DELAY_CTRL, + (1 << MPU9250_I2C_MST_DELAY_CTRL_DELAY_ES_SHADOW) | + (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV4_DLY_EN) | + (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV3_DLY_EN) | + (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV2_DLY_EN) | + (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV1_DLY_EN) | + (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV0_DLY_EN)); + + /* Set up i2c slave 0 to read the mag registers starting at HXL (3) */ + + _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_ADDR, + (1 << 7) | MPU9250_MAG_ADDR); + + _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_REG, + MPU9250_MAG_HXL); + + /* Byte swap so the mag values match the gyro/accel. Read 7 bytes + * to include the status register + */ + + _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_CTRL, + (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_EN) | + (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_BYTE_SW) | + (0 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_REG_DIS) | + (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_GRP) | + (MPU9250_MAG_ST2 - MPU9250_MAG_HXL + 1) << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_LENG); + /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */ _ao_mpu9250_reg_write(MPU9250_CONFIG, (MPU9250_CONFIG_FIFO_MODE_REPLACE << MPU9250_CONFIG_FIFO_MODE) | @@ -312,20 +430,15 @@ static void ao_mpu9250(void) { struct ao_mpu9250_sample sample; + /* ao_mpu9250_init already grabbed the SPI bus and mutex */ _ao_mpu9250_setup(); -#if AO_MPU9250_SPI ao_mpu9250_spi_put(); -#endif for (;;) { -#if AO_MPU9250_SPI ao_mpu9250_spi_get(); -#endif _ao_mpu9250_sample(&sample); -#if AO_MPU9250_SPI ao_mpu9250_spi_put(); -#endif ao_arch_block_interrupts(); ao_mpu9250_current = sample; AO_DATA_PRESENT(AO_DATA_MPU9250); @@ -339,15 +452,20 @@ static struct ao_task ao_mpu9250_task; static void ao_mpu9250_show(void) { - printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n", + printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n", ao_mpu9250_current.accel_x, ao_mpu9250_current.accel_y, ao_mpu9250_current.accel_z, ao_mpu9250_current.gyro_x, ao_mpu9250_current.gyro_y, - ao_mpu9250_current.gyro_z); + ao_mpu9250_current.gyro_z, + ao_mpu9250_current.mag_x, + ao_mpu9250_current.mag_y, + ao_mpu9250_current.mag_z); } +#if MPU9250_TEST + static void ao_mpu9250_read(void) { @@ -384,10 +502,52 @@ ao_mpu9250_write(void) ao_mpu9250_spi_put(); } +static void +ao_mpu9250_mag_read(void) +{ + uint8_t addr; + uint8_t val; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + addr = ao_cmd_lex_i; + ao_mpu9250_spi_get(); + val = _ao_mpu9250_mag_reg_read(addr); + ao_mpu9250_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_mpu9250_mag_write(void) +{ + uint8_t addr; + uint8_t val; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + addr = ao_cmd_lex_i; + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + val = ao_cmd_lex_i; + printf("Addr %02x val %02x\n", addr, val); + ao_mpu9250_spi_get(); + _ao_mpu9250_mag_reg_write(addr, val); + ao_mpu9250_spi_put(); +} + +#endif /* MPU9250_TEST */ + static const struct ao_cmds ao_mpu9250_cmds[] = { { ao_mpu9250_show, "I\0Show MPU9250 status" }, +#if MPU9250_TEST { ao_mpu9250_read, "R <addr>\0Read MPU9250 register" }, { ao_mpu9250_write, "W <addr> <val>\0Write MPU9250 register" }, + { ao_mpu9250_mag_read, "G <addr>\0Read MPU9250 Mag register" }, + { ao_mpu9250_mag_write, "P <addr> <val>\0Write MPU9250 Mag register" }, +#endif { 0, NULL } }; diff --git a/src/drivers/ao_mpu9250.h b/src/drivers/ao_mpu9250.h index df1be7c7..5e8e0885 100644 --- a/src/drivers/ao_mpu9250.h +++ b/src/drivers/ao_mpu9250.h @@ -130,6 +130,12 @@ #define MPU9250_I2C_SLV0_REG 0x26 #define MPU9250_I2C_SLV0_CTRL 0x27 +#define MPU9250_I2C_SLV0_CTRL_I2C_SLV0_EN 7 +#define MPU9250_I2C_SLV0_CTRL_I2C_SLV0_BYTE_SW 6 +#define MPU9250_I2C_SLV0_CTRL_I2C_SLV0_REG_DIS 5 +#define MPU9250_I2C_SLV0_CTRL_I2C_SLV0_GRP 4 +#define MPU9250_I2C_SLV0_CTRL_I2C_SLV0_LENG 0 + #define MPU9250_I2C_SLV1_ADDR 0x28 #define MPU9250_I2C_SLV1_REG 0x29 #define MPU9250_I2C_SLV1_CTRL 0x2a @@ -146,6 +152,11 @@ #define MPU9250_I2C_SLV4_REG 0x32 #define MPU9250_I2C_SLV4_DO 0x33 #define MPU9250_I2C_SLV4_CTRL 0x34 +#define MPU9250_I2C_SLV4_CTRL_I2C_SLV4_EN 7 +#define MPU9250_I2C_SLV4_CTRL_SLV4_DONE_INT_EN 6 +#define MPU9250_I2C_SLV4_CTRL_I2C_SLV4_REG_DIS 5 +#define MPU9250_I2C_SLV4_CTRL_I2C_MST_DLY 0 + #define MPU9250_I2C_SLV4_DI 0x35 #define MPU9250_I2C_MST_STATUS 0x36 @@ -179,6 +190,15 @@ #define MPU9250_GYRO_ZOUT_H 0x47 #define MPU9250_GYRO_ZOUT_L 0x48 +#define MPU9250_I2C_MST_DELAY_CTRL 0x67 + +#define MPU9250_I2C_MST_DELAY_CTRL_DELAY_ES_SHADOW 7 +#define MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV4_DLY_EN 4 +#define MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV3_DLY_EN 3 +#define MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV2_DLY_EN 2 +#define MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV1_DLY_EN 1 +#define MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV0_DLY_EN 0 + #define MPU9250_SIGNAL_PATH_RESET 0x68 #define MPU9250_SIGNAL_PATH_RESET_GYRO_RESET 2 #define MPU9250_SIGNAL_PATH_RESET_ACCEL_RESET 1 @@ -212,6 +232,57 @@ #define MPU9250_WHO_AM_I 0x75 #define MPU9250_I_AM_9250 0x71 +/* AK8963 mag sensor on the I2C bus */ + +#define MPU9250_MAG_ADDR 0x0c + +#define MPU9250_MAG_WIA 0x00 +#define MPU9250_MAG_WIA_VALUE 0x48 + +#define MPU9250_MAG_INFO 0x01 +#define MPU9250_MAG_ST1 0x02 +#define MPU9250_MAG_ST1_DOR 1 +#define MPU9250_MAG_ST1_DRDY 0 + +#define MPU9250_MAG_HXL 0x03 +#define MPU9250_MAG_HXH 0x04 +#define MPU9250_MAG_HYL 0x05 +#define MPU9250_MAG_HYH 0x06 +#define MPU9250_MAG_HZL 0x07 +#define MPU9250_MAG_HZH 0x08 +#define MPU9250_MAG_ST2 0x09 +#define MPU9250_MAG_ST2_BITM 4 +#define MPU9250_MAG_ST2_HOFL 3 + +#define MPU9250_MAG_CNTL1 0x0a +#define MPU9250_MAG_CNTL1_MODE 0 +#define MPU9250_MAG_CNTL1_MODE_POWER_DOWN 0x0 +#define MPU9250_MAG_CNTL1_MODE_SINGLE 0x1 +#define MPU9250_MAG_CNTL1_MODE_CONT_1 0x2 /* 8Hz */ +#define MPU9250_MAG_CNTL1_MODE_CONT_2 0x6 /* 100Hz */ +#define MPU9250_MAG_CNTL1_MODE_EXTERNAL 0x4 +#define MPU9250_MAG_CNTL1_MODE_SELF_TEST 0x8 +#define MPU9250_MAG_CNTL1_MODE_FUSE_ACCESS 0xf + +#define MPU9250_MAG_CNTL1_BIT 4 +#define MPU9250_MAG_CNTL1_BIT_14 0 +#define MPU9250_MAG_CNTL1_BIT_16 1 + +#define MPU9250_MAG_CNTL2 0x0b +#define MPU9250_MAG_CNTL2_SRST 0 + +#define MPU9250_MAG_ASTC 0x0c +#define MPU9250_MAG_ASTC_SELF 6 + +#define MPU9250_MAG_TS1 0x0d +#define MPU9250_MAG_TS2 0x0e +#define MPU9250_MAG_I2CDIS 0x0f +#define MPU9250_MAG_I2CDIS_VALUE 0x1d + +#define MPU9250_MAG_ASAX 0x10 +#define MPU9250_MAG_ASAY 0x11 +#define MPU9250_MAG_ASAZ 0x12 + /* Self test acceleration is approximately 0.5g */ #define MPU9250_ST_ACCEL(full_scale) (32767 / ((full_scale) * 2)) |