diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/ao_mpu9250.c | 575 | ||||
| -rw-r--r-- | src/drivers/ao_mpu9250.h | 340 |
2 files changed, 915 insertions, 0 deletions
diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c new file mode 100644 index 00000000..ae8dacd0 --- /dev/null +++ b/src/drivers/ao_mpu9250.c @@ -0,0 +1,575 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_mpu9250.h> +#include <ao_exti.h> + +#if HAS_MPU9250 + +#define MPU9250_TEST 0 + +static uint8_t ao_mpu9250_configured; + +extern uint8_t ao_sensor_errors; + +#ifndef AO_MPU9250_I2C_INDEX +#define AO_MPU9250_SPI 1 +#else +#define AO_MPU9250_SPI 0 +#endif + +#if AO_MPU9250_SPI + +#define ao_mpu9250_spi_get() ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz) +#define ao_mpu9250_spi_put() ao_spi_put(AO_MPU9250_SPI_BUS) + +#define ao_mpu9250_spi_start() ao_spi_set_cs(AO_MPU9250_SPI_CS_PORT, \ + (1 << AO_MPU9250_SPI_CS_PIN)) + +#define ao_mpu9250_spi_end() ao_spi_clr_cs(AO_MPU9250_SPI_CS_PORT, \ + (1 << AO_MPU9250_SPI_CS_PIN)) + +#else + +#define ao_mpu9250_spi_get() +#define ao_mpu9250_spi_put() + +#endif + +static void +_ao_mpu9250_reg_write(uint8_t addr, uint8_t value) +{ + uint8_t d[2] = { addr, value }; +#if AO_MPU9250_SPI + ao_mpu9250_spi_start(); + ao_spi_send(d, 2, AO_MPU9250_SPI_BUS); + ao_mpu9250_spi_end(); +#else + ao_i2c_get(AO_MPU9250_I2C_INDEX); + ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE); + ao_i2c_send(d, 2, AO_MPU9250_I2C_INDEX, TRUE); + ao_i2c_put(AO_MPU9250_I2C_INDEX); +#endif +} + +static void +_ao_mpu9250_read(uint8_t addr, void *data, uint8_t len) +{ +#if AO_MPU9250_SPI + addr |= 0x80; + ao_mpu9250_spi_start(); + ao_spi_send(&addr, 1, AO_MPU9250_SPI_BUS); + ao_spi_recv(data, len, AO_MPU9250_SPI_BUS); + ao_mpu9250_spi_end(); +#else + ao_i2c_get(AO_MPU9250_I2C_INDEX); + ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE); + ao_i2c_send(&addr, 1, AO_MPU9250_I2C_INDEX, FALSE); + ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_READ); + ao_i2c_recv(data, len, AO_MPU9250_I2C_INDEX, TRUE); + ao_i2c_put(AO_MPU9250_I2C_INDEX); +#endif +} + +static uint8_t +_ao_mpu9250_reg_read(uint8_t addr) +{ + uint8_t value; +#if AO_MPU9250_SPI + addr |= 0x80; + ao_mpu9250_spi_start(); + ao_spi_send(&addr, 1, AO_MPU9250_SPI_BUS); + ao_spi_recv(&value, 1, AO_MPU9250_SPI_BUS); + ao_mpu9250_spi_end(); +#else + ao_i2c_get(AO_MPU9250_I2C_INDEX); + ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE); + ao_i2c_send(&addr, 1, AO_MPU9250_I2C_INDEX, FALSE); + ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_READ); + ao_i2c_recv(&value, 1, AO_MPU9250_I2C_INDEX, TRUE); + ao_i2c_put(AO_MPU9250_I2C_INDEX); +#endif + return value; +} + +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; + int i = sizeof (*sample) / 2; + + _ao_mpu9250_read(MPU9250_ACCEL_XOUT_H, sample, sizeof (*sample)); +#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² */ + +#if 0 +static int16_t /* cm/s² */ +ao_mpu9250_accel(int16_t v) +{ + return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767); +} + +static int16_t /* deg*10/s */ +ao_mpu9250_gyro(int16_t v) +{ + return (int16_t) ((v * (int32_t) 20000) / 32767); +} +#endif + +static uint8_t +ao_mpu9250_accel_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < MPU9250_ST_ACCEL(16) / 4) { + return 1; + } + if (diff > MPU9250_ST_ACCEL(16) * 4) { + return 1; + } + return 0; +} + +static uint8_t +ao_mpu9250_gyro_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < 0) + diff = -diff; + if (diff < MPU9250_ST_GYRO(2000) / 4) { + return 1; + } + if (diff > MPU9250_ST_GYRO(2000) * 4) { + return 1; + } + return 0; +} + +static void +_ao_mpu9250_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_mpu9250_reg_read(MPU9250_WHO_AM_I) == MPU9250_I_AM_9250) + break; + } + if (i == 30) + ao_panic(AO_PANIC_SELF_TEST_MPU9250); +} + +#define ST_TRIES 10 +#define MAG_TRIES 10 + +static void +_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; + + _ao_mpu9250_wait_alive(); + + /* Reset the whole chip */ + + _ao_mpu9250_reg_write(MPU9250_PWR_MGMT_1, + (1 << MPU9250_PWR_MGMT_1_DEVICE_RESET)); + + /* Wait for it to reset. If we talk too quickly, it appears to get confused */ + + _ao_mpu9250_wait_alive(); + + /* Reset signal conditioning, disabling I2C on SPI systems */ + _ao_mpu9250_reg_write(MPU9250_USER_CTRL, + (0 << MPU9250_USER_CTRL_FIFO_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) | + (1 << MPU9250_USER_CTRL_SIG_COND_RESET)); + + while (_ao_mpu9250_reg_read(MPU9250_USER_CTRL) & (1 << MPU9250_USER_CTRL_SIG_COND_RESET)) + ao_delay(AO_MS_TO_TICKS(10)); + + /* Reset signal paths */ + _ao_mpu9250_reg_write(MPU9250_SIGNAL_PATH_RESET, + (1 << MPU9250_SIGNAL_PATH_RESET_GYRO_RESET) | + (1 << MPU9250_SIGNAL_PATH_RESET_ACCEL_RESET) | + (1 << MPU9250_SIGNAL_PATH_RESET_TEMP_RESET)); + + _ao_mpu9250_reg_write(MPU9250_SIGNAL_PATH_RESET, + (0 << MPU9250_SIGNAL_PATH_RESET_GYRO_RESET) | + (0 << MPU9250_SIGNAL_PATH_RESET_ACCEL_RESET) | + (0 << MPU9250_SIGNAL_PATH_RESET_TEMP_RESET)); + + /* Select clocks, disable sleep */ + _ao_mpu9250_reg_write(MPU9250_PWR_MGMT_1, + (0 << MPU9250_PWR_MGMT_1_DEVICE_RESET) | + (0 << MPU9250_PWR_MGMT_1_SLEEP) | + (0 << MPU9250_PWR_MGMT_1_CYCLE) | + (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); + + /* Disable filtering */ + _ao_mpu9250_reg_write(MPU9250_CONFIG, + (MPU9250_CONFIG_EXT_SYNC_SET_DISABLED << MPU9250_CONFIG_EXT_SYNC_SET) | + (MPU9250_CONFIG_DLPF_CFG_250 << MPU9250_CONFIG_DLPF_CFG)); + + for (st_tries = 0; st_tries < ST_TRIES; st_tries++) { + errors = 0; + + /* Configure accelerometer to +/-16G in self-test mode */ + _ao_mpu9250_reg_write(MPU9250_ACCEL_CONFIG, + (1 << MPU9250_ACCEL_CONFIG_XA_ST) | + (1 << MPU9250_ACCEL_CONFIG_YA_ST) | + (1 << MPU9250_ACCEL_CONFIG_ZA_ST) | + (MPU9250_ACCEL_CONFIG_AFS_SEL_16G << MPU9250_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s in self-test mode */ + _ao_mpu9250_reg_write(MPU9250_GYRO_CONFIG, + (1 << MPU9250_GYRO_CONFIG_XG_ST) | + (1 << MPU9250_GYRO_CONFIG_YG_ST) | + (1 << MPU9250_GYRO_CONFIG_ZG_ST) | + (MPU9250_GYRO_CONFIG_FS_SEL_2000 << MPU9250_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu9250_sample(&test_mode); + + /* Configure accelerometer to +/-16G */ + _ao_mpu9250_reg_write(MPU9250_ACCEL_CONFIG, + (0 << MPU9250_ACCEL_CONFIG_XA_ST) | + (0 << MPU9250_ACCEL_CONFIG_YA_ST) | + (0 << MPU9250_ACCEL_CONFIG_ZA_ST) | + (MPU9250_ACCEL_CONFIG_AFS_SEL_16G << MPU9250_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s */ + _ao_mpu9250_reg_write(MPU9250_GYRO_CONFIG, + (0 << MPU9250_GYRO_CONFIG_XG_ST) | + (0 << MPU9250_GYRO_CONFIG_YG_ST) | + (0 << MPU9250_GYRO_CONFIG_ZG_ST) | + (MPU9250_GYRO_CONFIG_FS_SEL_2000 << MPU9250_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu9250_sample(&normal_mode); + + errors += ao_mpu9250_accel_check(normal_mode.accel_x, test_mode.accel_x); + errors += ao_mpu9250_accel_check(normal_mode.accel_y, test_mode.accel_y); + errors += ao_mpu9250_accel_check(normal_mode.accel_z, test_mode.accel_z); + + errors += ao_mpu9250_gyro_check(normal_mode.gyro_x, test_mode.gyro_x); + errors += ao_mpu9250_gyro_check(normal_mode.gyro_y, test_mode.gyro_y); + errors += ao_mpu9250_gyro_check(normal_mode.gyro_z, test_mode.gyro_z); + if (!errors) + break; + } + + 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) | + (MPU9250_CONFIG_EXT_SYNC_SET_DISABLED << MPU9250_CONFIG_EXT_SYNC_SET) | + (MPU9250_CONFIG_DLPF_CFG_92 << MPU9250_CONFIG_DLPF_CFG)); + + /* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */ + _ao_mpu9250_reg_write(MPU9250_SMPRT_DIV, + 1000 / 200 - 1); + + ao_delay(AO_MS_TO_TICKS(100)); + ao_mpu9250_configured = 1; +} + +struct ao_mpu9250_sample ao_mpu9250_current; + +static void +ao_mpu9250(void) +{ + struct ao_mpu9250_sample sample; + + /* ao_mpu9250_init already grabbed the SPI bus and mutex */ + _ao_mpu9250_setup(); + ao_mpu9250_spi_put(); + for (;;) + { + ao_mpu9250_spi_get(); + _ao_mpu9250_sample(&sample); + ao_mpu9250_spi_put(); + ao_arch_block_interrupts(); + ao_mpu9250_current = sample; + AO_DATA_PRESENT(AO_DATA_MPU9250); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); + } +} + +static struct ao_task ao_mpu9250_task; + +static void +ao_mpu9250_show(void) +{ + 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.mag_x, + ao_mpu9250_current.mag_y, + ao_mpu9250_current.mag_z); +} + +#if MPU9250_TEST + +static void +ao_mpu9250_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_reg_read(addr); + ao_mpu9250_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_mpu9250_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_reg_write(addr, val); + 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 } +}; + +void +ao_mpu9250_init(void) +{ + ao_mpu9250_configured = 0; + + ao_add_task(&ao_mpu9250_task, ao_mpu9250, "mpu9250"); + +#if AO_MPU9250_SPI + ao_spi_init_cs(AO_MPU9250_SPI_CS_PORT, (1 << AO_MPU9250_SPI_CS_PIN)); + + /* Pretend to be the mpu9250 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_mpu9250_task; + ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz); + ao_cur_task = NULL; +#endif + ao_cmd_register(&ao_mpu9250_cmds[0]); +} +#endif diff --git a/src/drivers/ao_mpu9250.h b/src/drivers/ao_mpu9250.h new file mode 100644 index 00000000..5e8e0885 --- /dev/null +++ b/src/drivers/ao_mpu9250.h @@ -0,0 +1,340 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_MPU9250_H_ +#define _AO_MPU9250_H_ + +#ifndef M_PI +#define M_PI 3.1415926535897832384626433 +#endif + +#define MPU9250_ADDR_WRITE 0xd0 +#define MPU9250_ADDR_READ 0xd1 + +/* From Tridge */ +#define MPUREG_XG_OFFS_TC 0x00 +#define MPUREG_YG_OFFS_TC 0x01 +#define MPUREG_ZG_OFFS_TC 0x02 +#define MPUREG_X_FINE_GAIN 0x03 +#define MPUREG_Y_FINE_GAIN 0x04 +#define MPUREG_Z_FINE_GAIN 0x05 +#define MPUREG_XA_OFFS_H 0x06 // X axis accelerometer offset (high byte) +#define MPUREG_XA_OFFS_L 0x07 // X axis accelerometer offset (low byte) +#define MPUREG_YA_OFFS_H 0x08 // Y axis accelerometer offset (high byte) +#define MPUREG_YA_OFFS_L 0x09 // Y axis accelerometer offset (low byte) +#define MPUREG_ZA_OFFS_H 0x0A // Z axis accelerometer offset (high byte) +#define MPUREG_ZA_OFFS_L 0x0B // Z axis accelerometer offset (low byte) +#define MPUREG_PRODUCT_ID 0x0C // Product ID Register +#define MPUREG_XG_OFFS_USRH 0x13 // X axis gyro offset (high byte) +#define MPUREG_XG_OFFS_USRL 0x14 // X axis gyro offset (low byte) +#define MPUREG_YG_OFFS_USRH 0x15 // Y axis gyro offset (high byte) +#define MPUREG_YG_OFFS_USRL 0x16 // Y axis gyro offset (low byte) +#define MPUREG_ZG_OFFS_USRH 0x17 // Z axis gyro offset (high byte) +#define MPUREG_ZG_OFFS_USRL 0x18 // Z axis gyro offset (low byte) + +#define MPU9250_SMPRT_DIV 0x19 + +#define MPU9250_CONFIG 0x1a + +#define MPU9250_CONFIG_FIFO_MODE 6 +# define MPU9250_CONFIG_FIFO_MODE_REPLACE 0 +# define MPU9250_CONFIG_FIFO_MODE_DROP 1 + +#define MPU9250_CONFIG_EXT_SYNC_SET 3 +#define MPU9250_CONFIG_EXT_SYNC_SET_DISABLED 0 +#define MPU9250_CONFIG_EXT_SYNC_SET_TEMP_OUT_L 1 +#define MPU9250_CONFIG_EXT_SYNC_SET_GYRO_XOUT_L 2 +#define MPU9250_CONFIG_EXT_SYNC_SET_GYRO_YOUT_L 3 +#define MPU9250_CONFIG_EXT_SYNC_SET_GYRO_ZOUT_L 4 +#define MPU9250_CONFIG_EXT_SYNC_SET_ACCEL_XOUT_L 5 +#define MPU9250_CONFIG_EXT_SYNC_SET_ACCEL_YOUT_L 6 +#define MPU9250_CONFIG_EXT_SYNC_SET_ACCEL_ZOUT_L 7 +#define MPU9250_CONFIG_EXT_SYNC_SET_MASK 7 + +#define MPU9250_CONFIG_DLPF_CFG 0 +#define MPU9250_CONFIG_DLPF_CFG_250 0 +#define MPU9250_CONFIG_DLPF_CFG_184 1 +#define MPU9250_CONFIG_DLPF_CFG_92 2 +#define MPU9250_CONFIG_DLPF_CFG_41 3 +#define MPU9250_CONFIG_DLPF_CFG_20 4 +#define MPU9250_CONFIG_DLPF_CFG_10 5 +#define MPU9250_CONFIG_DLPF_CFG_5 6 +#define MPU9250_CONFIG_DLPF_CFG_MASK 7 + +#define MPU9250_GYRO_CONFIG 0x1b +# define MPU9250_GYRO_CONFIG_XG_ST 7 +# define MPU9250_GYRO_CONFIG_YG_ST 6 +# define MPU9250_GYRO_CONFIG_ZG_ST 5 +# define MPU9250_GYRO_CONFIG_FS_SEL 3 +# define MPU9250_GYRO_CONFIG_FS_SEL_250 0 +# define MPU9250_GYRO_CONFIG_FS_SEL_500 1 +# define MPU9250_GYRO_CONFIG_FS_SEL_1000 2 +# define MPU9250_GYRO_CONFIG_FS_SEL_2000 3 +# define MPU9250_GYRO_CONFIG_FS_SEL_MASK 3 +# define MPU9250_GYRO_CONFIG_FCHOICE_B 0 +# define MPU9250_GYRO_CONFIG_FCHOICE_B_8800 3 +# define MPU9250_GYRO_CONFIG_FCHOICE_B_3600 2 +# define MPU9250_GYRO_CONFIG_FCHOICE_B_LOW 0 + +#define MPU9250_ACCEL_CONFIG 0x1c +# define MPU9250_ACCEL_CONFIG_XA_ST 7 +# define MPU9250_ACCEL_CONFIG_YA_ST 6 +# define MPU9250_ACCEL_CONFIG_ZA_ST 5 +# define MPU9250_ACCEL_CONFIG_AFS_SEL 3 +# define MPU9250_ACCEL_CONFIG_AFS_SEL_2G 0 +# define MPU9250_ACCEL_CONFIG_AFS_SEL_4G 1 +# define MPU9250_ACCEL_CONFIG_AFS_SEL_8G 2 +# define MPU9250_ACCEL_CONFIG_AFS_SEL_16G 3 +# define MPU9250_ACCEL_CONFIG_AFS_SEL_MASK 3 + +#define MPU9250_MST_CTRL 0x24 +#define MPU9250_MST_CTRL_MULT_MST_EN 7 +#define MPU9250_MST_CTRL_WAIT_FOR_ES 6 +#define MPU9250_MST_CTRL_SLV_3_FIFO_EN 5 +#define MPU9250_MST_CTRL_I2C_MST_P_NSR 4 +#define MPU9250_MST_CTRL_I2C_MST_CLK 0 +#define MPU9250_MST_CTRL_I2C_MST_CLK_348 0 +#define MPU9250_MST_CTRL_I2C_MST_CLK_333 1 +#define MPU9250_MST_CTRL_I2C_MST_CLK_320 2 +#define MPU9250_MST_CTRL_I2C_MST_CLK_308 3 +#define MPU9250_MST_CTRL_I2C_MST_CLK_296 4 +#define MPU9250_MST_CTRL_I2C_MST_CLK_286 5 +#define MPU9250_MST_CTRL_I2C_MST_CLK_276 6 +#define MPU9250_MST_CTRL_I2C_MST_CLK_267 7 +#define MPU9250_MST_CTRL_I2C_MST_CLK_258 8 +#define MPU9250_MST_CTRL_I2C_MST_CLK_500 9 +#define MPU9250_MST_CTRL_I2C_MST_CLK_471 10 +#define MPU9250_MST_CTRL_I2C_MST_CLK_444 11 +#define MPU9250_MST_CTRL_I2C_MST_CLK_421 12 +#define MPU9250_MST_CTRL_I2C_MST_CLK_400 13 +#define MPU9250_MST_CTRL_I2C_MST_CLK_381 14 +#define MPU9250_MST_CTRL_I2C_MST_CLK_364 15 +#define MPU9250_MST_CTRL_I2C_MST_CLK_MASK 15 + +#define MPU9250_I2C_SLV0_ADDR 0x25 +#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 + +#define MPU9250_I2C_SLV2_ADDR 0x2b +#define MPU9250_I2C_SLV2_REG 0x2c +#define MPU9250_I2C_SLV2_CTRL 0x2d + +#define MPU9250_I2C_SLV3_ADDR 0x2e +#define MPU9250_I2C_SLV3_REG 0x2f +#define MPU9250_I2C_SLV3_CTRL 0x30 + +#define MPU9250_I2C_SLV4_ADDR 0x31 +#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 + +#define MPU9250_INT_PIN_CFG 0x37 + +#define MPU9250_INT_ENABLE 0x38 +#define MPU9250_INT_ENABLE_WOM_EN 6 +#define MPU9250_INT_ENABLE_FIFO_OFLOW_EN 4 +#define MPU9250_INT_ENABLE_FSYNC_INT_EN 3 +#define MPU9250_INT_ENABLE_RAW_RDY_EN 0 + +#define MPU9250_INT_STATUS 0x3a +#define MPU9250_INT_STATUS_WOM_INT 6 +#define MPU9250_INT_STATUS_FIFO_OFLOW_INT 4 +#define MPU9250_INT_STATUS_FSYNC_INT 3 +#define MPU9250_INT_STATUS_RAW_RDY_INT 0 + +#define MPU9250_ACCEL_XOUT_H 0x3b +#define MPU9250_ACCEL_XOUT_L 0x3c +#define MPU9250_ACCEL_YOUT_H 0x3d +#define MPU9250_ACCEL_YOUT_L 0x3e +#define MPU9250_ACCEL_ZOUT_H 0x3f +#define MPU9250_ACCEL_ZOUT_L 0x40 +#define MPU9250_TEMP_H 0x41 +#define MPU9250_TEMP_L 0x42 +#define MPU9250_GYRO_XOUT_H 0x43 +#define MPU9250_GYRO_XOUT_L 0x44 +#define MPU9250_GYRO_YOUT_H 0x45 +#define MPU9250_GYRO_YOUT_L 0x46 +#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 +#define MPU9250_SIGNAL_PATH_RESET_TEMP_RESET 0 + +#define MPU9250_USER_CTRL 0x6a +#define MPU9250_USER_CTRL_FIFO_EN 6 +#define MPU9250_USER_CTRL_I2C_MST_EN 5 +#define MPU9250_USER_CTRL_I2C_IF_DIS 4 +#define MPU9250_USER_CTRL_FIFO_RESET 2 +#define MPU9250_USER_CTRL_I2C_MST_RESET 1 +#define MPU9250_USER_CTRL_SIG_COND_RESET 0 + +#define MPU9250_PWR_MGMT_1 0x6b +#define MPU9250_PWR_MGMT_1_DEVICE_RESET 7 +#define MPU9250_PWR_MGMT_1_SLEEP 6 +#define MPU9250_PWR_MGMT_1_CYCLE 5 +#define MPU9250_PWR_MGMT_1_TEMP_DIS 3 +#define MPU9250_PWR_MGMT_1_CLKSEL 0 +#define MPU9250_PWR_MGMT_1_CLKSEL_INTERNAL 0 +#define MPU9250_PWR_MGMT_1_CLKSEL_PLL_X_AXIS 1 +#define MPU9250_PWR_MGMT_1_CLKSEL_PLL_Y_AXIS 2 +#define MPU9250_PWR_MGMT_1_CLKSEL_PLL_Z_AXIS 3 +#define MPU9250_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_32K 4 +#define MPU9250_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_19M 5 +#define MPU9250_PWR_MGMT_1_CLKSEL_STOP 7 +#define MPU9250_PWR_MGMT_1_CLKSEL_MASK 7 + +#define MPU9250_PWR_MGMT_2 0x6c + +#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)) + +/* Self test gyro is approximately 50°/s */ +#define MPU9250_ST_GYRO(full_scale) ((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale))) + +#define MPU9250_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) + +static inline float +ao_mpu9250_gyro(float sensor) { + return sensor * ((float) (MPU9250_GYRO_FULLSCALE / 32767.0)); +} + +#define MPU9250_ACCEL_FULLSCALE 16 + +static inline float +ao_mpu9250_accel(int16_t sensor) { + return (float) sensor * ((float) (MPU9250_ACCEL_FULLSCALE * GRAVITY / 32767.0)); +} + +struct ao_mpu9250_sample { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + int16_t mag_x; + int16_t mag_y; + int16_t mag_z; +}; + +extern struct ao_mpu9250_sample ao_mpu9250_current; + +void +ao_mpu9250_init(void); + +/* Product ID Description for MPU9250 + * high 4 bits low 4 bits + * Product Name Product Revision + */ +#define MPU9250ES_REV_C4 0x14 /* 0001 0100 */ +#define MPU9250ES_REV_C5 0x15 /* 0001 0101 */ +#define MPU9250ES_REV_D6 0x16 /* 0001 0110 */ +#define MPU9250ES_REV_D7 0x17 /* 0001 0111 */ +#define MPU9250ES_REV_D8 0x18 /* 0001 1000 */ +#define MPU9250_REV_C4 0x54 /* 0101 0100 */ +#define MPU9250_REV_C5 0x55 /* 0101 0101 */ +#define MPU9250_REV_D6 0x56 /* 0101 0110 */ +#define MPU9250_REV_D7 0x57 /* 0101 0111 */ +#define MPU9250_REV_D8 0x58 /* 0101 1000 */ +#define MPU9250_REV_D9 0x59 /* 0101 1001 */ + +#endif /* _AO_MPU9250_H_ */ |