It is modified accordingly to work with sparkfun dmp library under mbed platform
Dependents: MPU9250-dmp-bluepill MPU9250-dmp
Fork of MotionDriver_6_1 by
Diff: inv_mpu.c
- Revision:
- 0:5fa30cf392c3
- Child:
- 1:a6c3f8680fe0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inv_mpu.c Sun Aug 24 00:52:07 2014 +0000 @@ -0,0 +1,3291 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu.c + * @brief An I2C-based driver for Invensense gyroscopes. + * @details This driver currently works for the following devices: + * MPU6050 + * MPU6500 + * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) + * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) + */ +#include <stdio.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include "inv_mpu.h" + +/* The following functions must be defined for this platform: + * i2c_write(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char const *data) + * i2c_read(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char *data) + * delay_ms(unsigned long num_ms) + * get_ms(unsigned long *count) + * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) + * labs(long x) + * fabsf(float x) + * min(int a, int b) + */ +#if defined EMPL_TARGET_STM32F4 +#include "i2c.h" +#include "main.h" +#include "log.h" +#include "board-st_discovery.h" + +#define i2c_write Sensors_I2C_WriteRegister +#define i2c_read Sensors_I2C_ReadRegister +#define delay_ms mdelay +#define get_ms get_tick_count +#define log_i MPL_LOGI +#define log_e MPL_LOGE +#define min(a,b) ((a<b)?a:b) + +#elif defined MOTION_DRIVER_TARGET_MSP430 +#include "msp430.h" +#include "msp430_i2c.h" +#include "msp430_clock.h" +#include "msp430_interrupt.h" +#define i2c_write msp430_i2c_write +#define i2c_read msp430_i2c_read +#define delay_ms msp430_delay_ms +#define get_ms msp430_get_clock_ms +static inline int reg_int_cb(struct int_param_s *int_param) +{ + return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, + int_param->active_low); +} +#define log_i(...) do {} while (0) +#define log_e(...) do {} while (0) +/* labs is already defined by TI's toolchain. */ +/* fabs is for doubles. fabsf is for floats. */ +#define fabs fabsf +#define min(a,b) ((a<b)?a:b) +#elif defined EMPL_TARGET_MSP430 +#include "msp430.h" +#include "msp430_i2c.h" +#include "msp430_clock.h" +#include "msp430_interrupt.h" +#include "log.h" +#define i2c_write msp430_i2c_write +#define i2c_read msp430_i2c_read +#define delay_ms msp430_delay_ms +#define get_ms msp430_get_clock_ms +static inline int reg_int_cb(struct int_param_s *int_param) +{ + return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, + int_param->active_low); +} +#define log_i MPL_LOGI +#define log_e MPL_LOGE +/* labs is already defined by TI's toolchain. */ +/* fabs is for doubles. fabsf is for floats. */ +#define fabs fabsf +#define min(a,b) ((a<b)?a:b) +#elif defined EMPL_TARGET_UC3L0 +/* Instead of using the standard TWI driver from the ASF library, we're using + * a TWI driver that follows the slave address + register address convention. + */ +#include "twi.h" +#include "delay.h" +#include "sysclk.h" +#include "log.h" +#include "sensors_xplained.h" +#include "uc3l0_clock.h" +#define i2c_write(a, b, c, d) twi_write(a, b, d, c) +#define i2c_read(a, b, c, d) twi_read(a, b, d, c) +/* delay_ms is a function already defined in ASF. */ +#define get_ms uc3l0_get_clock_ms +static inline int reg_int_cb(struct int_param_s *int_param) +{ + sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg); + return 0; +} +#define log_i MPL_LOGI +#define log_e MPL_LOGE +/* UC3 is a 32-bit processor, so abs and labs are equivalent. */ +#define labs abs +#define fabs(x) (((x)>0)?(x):-(x)) +#else +#error Gyro driver is missing the system layer implementations. +#endif + +#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 +#error Which gyro are you using? Define MPUxxxx in your compiler options. +#endif + +/* Time for some messy macro work. =] + * #define MPU9150 + * is equivalent to.. + * #define MPU6050 + * #define AK8975_SECONDARY + * + * #define MPU9250 + * is equivalent to.. + * #define MPU6500 + * #define AK8963_SECONDARY + */ +#if defined MPU9150 +#ifndef MPU6050 +#define MPU6050 +#endif /* #ifndef MPU6050 */ +#if defined AK8963_SECONDARY +#error "MPU9150 and AK8963_SECONDARY cannot both be defined." +#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ +#define AK8975_SECONDARY +#endif /* #if defined AK8963_SECONDARY */ +#elif defined MPU9250 /* #if defined MPU9150 */ +#ifndef MPU6500 +#define MPU6500 +#endif /* #ifndef MPU6500 */ +#if defined AK8975_SECONDARY +#error "MPU9250 and AK8975_SECONDARY cannot both be defined." +#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ +#define AK8963_SECONDARY +#endif /* #if defined AK8975_SECONDARY */ +#endif /* #if defined MPU9150 */ + +#if defined AK8975_SECONDARY || defined AK8963_SECONDARY +#define AK89xx_SECONDARY +#else +/* #warning "No compass = less profit for Invensense. Lame." */ +#endif + +static int set_int_enable(unsigned char enable); + +/* Hardware registers needed by driver. */ +struct gyro_reg_s { + unsigned char who_am_i; + unsigned char rate_div; + unsigned char lpf; + unsigned char prod_id; + unsigned char user_ctrl; + unsigned char fifo_en; + unsigned char gyro_cfg; + unsigned char accel_cfg; + unsigned char accel_cfg2; + unsigned char lp_accel_odr; + unsigned char motion_thr; + unsigned char motion_dur; + unsigned char fifo_count_h; + unsigned char fifo_r_w; + unsigned char raw_gyro; + unsigned char raw_accel; + unsigned char temp; + unsigned char int_enable; + unsigned char dmp_int_status; + unsigned char int_status; + unsigned char accel_intel; + unsigned char pwr_mgmt_1; + unsigned char pwr_mgmt_2; + unsigned char int_pin_cfg; + unsigned char mem_r_w; + unsigned char accel_offs; + unsigned char i2c_mst; + unsigned char bank_sel; + unsigned char mem_start_addr; + unsigned char prgm_start_h; +#if defined AK89xx_SECONDARY + unsigned char s0_addr; + unsigned char s0_reg; + unsigned char s0_ctrl; + unsigned char s1_addr; + unsigned char s1_reg; + unsigned char s1_ctrl; + unsigned char s4_ctrl; + unsigned char s0_do; + unsigned char s1_do; + unsigned char i2c_delay_ctrl; + unsigned char raw_compass; + /* The I2C_MST_VDDIO bit is in this register. */ + unsigned char yg_offs_tc; +#endif +}; + +/* Information specific to a particular device. */ +struct hw_s { + unsigned char addr; + unsigned short max_fifo; + unsigned char num_reg; + unsigned short temp_sens; + short temp_offset; + unsigned short bank_size; +#if defined AK89xx_SECONDARY + unsigned short compass_fsr; +#endif +}; + +/* When entering motion interrupt mode, the driver keeps track of the + * previous state so that it can be restored at a later time. + * TODO: This is tacky. Fix it. + */ +struct motion_int_cache_s { + unsigned short gyro_fsr; + unsigned char accel_fsr; + unsigned short lpf; + unsigned short sample_rate; + unsigned char sensors_on; + unsigned char fifo_sensors; + unsigned char dmp_on; +}; + +/* Cached chip configuration data. + * TODO: A lot of these can be handled with a bitmask. + */ +struct chip_cfg_s { + /* Matches gyro_cfg >> 3 & 0x03 */ + unsigned char gyro_fsr; + /* Matches accel_cfg >> 3 & 0x03 */ + unsigned char accel_fsr; + /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ + unsigned char sensors; + /* Matches config register. */ + unsigned char lpf; + unsigned char clk_src; + /* Sample rate, NOT rate divider. */ + unsigned short sample_rate; + /* Matches fifo_en register. */ + unsigned char fifo_enable; + /* Matches int enable register. */ + unsigned char int_enable; + /* 1 if devices on auxiliary I2C bus appear on the primary. */ + unsigned char bypass_mode; + /* 1 if half-sensitivity. + * NOTE: This doesn't belong here, but everything else in hw_s is const, + * and this allows us to save some precious RAM. + */ + unsigned char accel_half; + /* 1 if device in low-power accel-only mode. */ + unsigned char lp_accel_mode; + /* 1 if interrupts are only triggered on motion events. */ + unsigned char int_motion_only; + struct motion_int_cache_s cache; + /* 1 for active low interrupts. */ + unsigned char active_low_int; + /* 1 for latched interrupts. */ + unsigned char latched_int; + /* 1 if DMP is enabled. */ + unsigned char dmp_on; + /* Ensures that DMP will only be loaded once. */ + unsigned char dmp_loaded; + /* Sampling rate used when DMP is enabled. */ + unsigned short dmp_sample_rate; +#ifdef AK89xx_SECONDARY + /* Compass sample rate. */ + unsigned short compass_sample_rate; + unsigned char compass_addr; + short mag_sens_adj[3]; +#endif +}; + +/* Information for self-test. */ +struct test_s { + unsigned long gyro_sens; + unsigned long accel_sens; + unsigned char reg_rate_div; + unsigned char reg_lpf; + unsigned char reg_gyro_fsr; + unsigned char reg_accel_fsr; + unsigned short wait_ms; + unsigned char packet_thresh; + float min_dps; + float max_dps; + float max_gyro_var; + float min_g; + float max_g; + float max_accel_var; +#ifdef MPU6500 + float max_g_offset; + unsigned short sample_wait_ms; +#endif +}; + +/* Gyro driver state variables. */ +struct gyro_state_s { + const struct gyro_reg_s *reg; + const struct hw_s *hw; + struct chip_cfg_s chip_cfg; + const struct test_s *test; +}; + +/* Filter configurations. */ +enum lpf_e { + INV_FILTER_256HZ_NOLPF2 = 0, + INV_FILTER_188HZ, + INV_FILTER_98HZ, + INV_FILTER_42HZ, + INV_FILTER_20HZ, + INV_FILTER_10HZ, + INV_FILTER_5HZ, + INV_FILTER_2100HZ_NOLPF, + NUM_FILTER +}; + +/* Full scale ranges. */ +enum gyro_fsr_e { + INV_FSR_250DPS = 0, + INV_FSR_500DPS, + INV_FSR_1000DPS, + INV_FSR_2000DPS, + NUM_GYRO_FSR +}; + +/* Full scale ranges. */ +enum accel_fsr_e { + INV_FSR_2G = 0, + INV_FSR_4G, + INV_FSR_8G, + INV_FSR_16G, + NUM_ACCEL_FSR +}; + +/* Clock sources. */ +enum clock_sel_e { + INV_CLK_INTERNAL = 0, + INV_CLK_PLL, + NUM_CLK +}; + +/* Low-power accel wakeup rates. */ +enum lp_accel_rate_e { +#if defined MPU6050 + INV_LPA_1_25HZ, + INV_LPA_5HZ, + INV_LPA_20HZ, + INV_LPA_40HZ +#elif defined MPU6500 + INV_LPA_0_3125HZ, + INV_LPA_0_625HZ, + INV_LPA_1_25HZ, + INV_LPA_2_5HZ, + INV_LPA_5HZ, + INV_LPA_10HZ, + INV_LPA_20HZ, + INV_LPA_40HZ, + INV_LPA_80HZ, + INV_LPA_160HZ, + INV_LPA_320HZ, + INV_LPA_640HZ +#endif +}; + +#define BIT_I2C_MST_VDDIO (0x80) +#define BIT_FIFO_EN (0x40) +#define BIT_DMP_EN (0x80) +#define BIT_FIFO_RST (0x04) +#define BIT_DMP_RST (0x08) +#define BIT_FIFO_OVERFLOW (0x10) +#define BIT_DATA_RDY_EN (0x01) +#define BIT_DMP_INT_EN (0x02) +#define BIT_MOT_INT_EN (0x40) +#define BITS_FSR (0x18) +#define BITS_LPF (0x07) +#define BITS_HPF (0x07) +#define BITS_CLK (0x07) +#define BIT_FIFO_SIZE_1024 (0x40) +#define BIT_FIFO_SIZE_2048 (0x80) +#define BIT_FIFO_SIZE_4096 (0xC0) +#define BIT_RESET (0x80) +#define BIT_SLEEP (0x40) +#define BIT_S0_DELAY_EN (0x01) +#define BIT_S2_DELAY_EN (0x04) +#define BITS_SLAVE_LENGTH (0x0F) +#define BIT_SLAVE_BYTE_SW (0x40) +#define BIT_SLAVE_GROUP (0x10) +#define BIT_SLAVE_EN (0x80) +#define BIT_I2C_READ (0x80) +#define BITS_I2C_MASTER_DLY (0x1F) +#define BIT_AUX_IF_EN (0x20) +#define BIT_ACTL (0x80) +#define BIT_LATCH_EN (0x20) +#define BIT_ANY_RD_CLR (0x10) +#define BIT_BYPASS_EN (0x02) +#define BITS_WOM_EN (0xC0) +#define BIT_LPA_CYCLE (0x20) +#define BIT_STBY_XA (0x20) +#define BIT_STBY_YA (0x10) +#define BIT_STBY_ZA (0x08) +#define BIT_STBY_XG (0x04) +#define BIT_STBY_YG (0x02) +#define BIT_STBY_ZG (0x01) +#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) +#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) + +#if defined AK8975_SECONDARY +#define SUPPORTS_AK89xx_HIGH_SENS (0x00) +#define AK89xx_FSR (9830) +#elif defined AK8963_SECONDARY +#define SUPPORTS_AK89xx_HIGH_SENS (0x10) +#define AK89xx_FSR (4915) +#endif + +#ifdef AK89xx_SECONDARY +#define AKM_REG_WHOAMI (0x00) + +#define AKM_REG_ST1 (0x02) +#define AKM_REG_HXL (0x03) +#define AKM_REG_ST2 (0x09) + +#define AKM_REG_CNTL (0x0A) +#define AKM_REG_ASTC (0x0C) +#define AKM_REG_ASAX (0x10) +#define AKM_REG_ASAY (0x11) +#define AKM_REG_ASAZ (0x12) + +#define AKM_DATA_READY (0x01) +#define AKM_DATA_OVERRUN (0x02) +#define AKM_OVERFLOW (0x80) +#define AKM_DATA_ERROR (0x40) + +#define AKM_BIT_SELF_TEST (0x40) + +#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) + +#define AKM_WHOAMI (0x48) +#endif + +#if defined MPU6050 +const struct gyro_reg_s reg = { + .who_am_i = 0x75, + .rate_div = 0x19, + .lpf = 0x1A, + .prod_id = 0x0C, + .user_ctrl = 0x6A, + .fifo_en = 0x23, + .gyro_cfg = 0x1B, + .accel_cfg = 0x1C, + .motion_thr = 0x1F, + .motion_dur = 0x20, + .fifo_count_h = 0x72, + .fifo_r_w = 0x74, + .raw_gyro = 0x43, + .raw_accel = 0x3B, + .temp = 0x41, + .int_enable = 0x38, + .dmp_int_status = 0x39, + .int_status = 0x3A, + .pwr_mgmt_1 = 0x6B, + .pwr_mgmt_2 = 0x6C, + .int_pin_cfg = 0x37, + .mem_r_w = 0x6F, + .accel_offs = 0x06, + .i2c_mst = 0x24, + .bank_sel = 0x6D, + .mem_start_addr = 0x6E, + .prgm_start_h = 0x70 +#ifdef AK89xx_SECONDARY + ,.raw_compass = 0x49, + .yg_offs_tc = 0x01, + .s0_addr = 0x25, + .s0_reg = 0x26, + .s0_ctrl = 0x27, + .s1_addr = 0x28, + .s1_reg = 0x29, + .s1_ctrl = 0x2A, + .s4_ctrl = 0x34, + .s0_do = 0x63, + .s1_do = 0x64, + .i2c_delay_ctrl = 0x67 +#endif +}; +const struct hw_s hw = { + .addr = 0x68, + .max_fifo = 1024, + .num_reg = 118, + .temp_sens = 340, + .temp_offset = -521, + .bank_size = 256 +#if defined AK89xx_SECONDARY + ,.compass_fsr = AK89xx_FSR +#endif +}; + +const struct test_s test = { + .gyro_sens = 32768/250, + .accel_sens = 32768/16, + .reg_rate_div = 0, /* 1kHz. */ + .reg_lpf = 1, /* 188Hz. */ + .reg_gyro_fsr = 0, /* 250dps. */ + .reg_accel_fsr = 0x18, /* 16g. */ + .wait_ms = 50, + .packet_thresh = 5, /* 5% */ + .min_dps = 10.f, + .max_dps = 105.f, + .max_gyro_var = 0.14f, + .min_g = 0.3f, + .max_g = 0.95f, + .max_accel_var = 0.14f +}; + +static struct gyro_state_s st = { + .reg = ®, + .hw = &hw, + .test = &test +}; +#elif defined MPU6500 +const struct gyro_reg_s reg = { + .who_am_i = 0x75, + .rate_div = 0x19, + .lpf = 0x1A, + .prod_id = 0x0C, + .user_ctrl = 0x6A, + .fifo_en = 0x23, + .gyro_cfg = 0x1B, + .accel_cfg = 0x1C, + .accel_cfg2 = 0x1D, + .lp_accel_odr = 0x1E, + .motion_thr = 0x1F, + .motion_dur = 0x20, + .fifo_count_h = 0x72, + .fifo_r_w = 0x74, + .raw_gyro = 0x43, + .raw_accel = 0x3B, + .temp = 0x41, + .int_enable = 0x38, + .dmp_int_status = 0x39, + .int_status = 0x3A, + .accel_intel = 0x69, + .pwr_mgmt_1 = 0x6B, + .pwr_mgmt_2 = 0x6C, + .int_pin_cfg = 0x37, + .mem_r_w = 0x6F, + .accel_offs = 0x77, + .i2c_mst = 0x24, + .bank_sel = 0x6D, + .mem_start_addr = 0x6E, + .prgm_start_h = 0x70 +#ifdef AK89xx_SECONDARY + ,.raw_compass = 0x49, + .s0_addr = 0x25, + .s0_reg = 0x26, + .s0_ctrl = 0x27, + .s1_addr = 0x28, + .s1_reg = 0x29, + .s1_ctrl = 0x2A, + .s4_ctrl = 0x34, + .s0_do = 0x63, + .s1_do = 0x64, + .i2c_delay_ctrl = 0x67 +#endif +}; +const struct hw_s hw = { + .addr = 0x68, + .max_fifo = 1024, + .num_reg = 128, + .temp_sens = 321, + .temp_offset = 0, + .bank_size = 256 +#if defined AK89xx_SECONDARY + ,.compass_fsr = AK89xx_FSR +#endif +}; + +const struct test_s test = { + .gyro_sens = 32768/250, + .accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G + .reg_rate_div = 0, /* 1kHz. */ + .reg_lpf = 2, /* 92Hz low pass filter*/ + .reg_gyro_fsr = 0, /* 250dps. */ + .reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */ + .wait_ms = 200, //200ms stabilization time + .packet_thresh = 200, /* 200 samples */ + .min_dps = 20.f, //20 dps for Gyro Criteria C + .max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B + .max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A + .min_g = .225f, //Accel must exceed Min 225 mg for Criteria B + .max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B + .max_accel_var = .5f, //Accel must be within 50% variation for Criteria A + .max_g_offset = .5f, //500 mg for Accel Criteria C + .sample_wait_ms = 10 //10ms sample time wait +}; + +static struct gyro_state_s st = { + .reg = ®, + .hw = &hw, + .test = &test +}; +#endif + +#define MAX_PACKET_LENGTH (12) +#ifdef MPU6500 +#define HWST_MAX_PACKET_LENGTH (512) +#endif + +#ifdef AK89xx_SECONDARY +static int setup_compass(void); +#define MAX_COMPASS_SAMPLE_RATE (100) +#endif + +/** + * @brief Enable/disable data ready interrupt. + * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready + * interrupt is used. + * @param[in] enable 1 to enable interrupt. + * @return 0 if successful. + */ +static int set_int_enable(unsigned char enable) +{ + unsigned char tmp; + + if (st.chip_cfg.dmp_on) { + if (enable) + tmp = BIT_DMP_INT_EN; + else + tmp = 0x00; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) + return -1; + st.chip_cfg.int_enable = tmp; + } else { + if (!st.chip_cfg.sensors) + return -1; + if (enable && st.chip_cfg.int_enable) + return 0; + if (enable) + tmp = BIT_DATA_RDY_EN; + else + tmp = 0x00; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) + return -1; + st.chip_cfg.int_enable = tmp; + } + return 0; +} + +/** + * @brief Register dump for testing. + * @return 0 if successful. + */ +int mpu_reg_dump(void) +{ + unsigned char ii; + unsigned char data; + + for (ii = 0; ii < st.hw->num_reg; ii++) { + if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) + continue; + if (i2c_read(st.hw->addr, ii, 1, &data)) + return -1; + log_i("%#5x: %#5x\r\n", ii, data); + } + return 0; +} + +/** + * @brief Read from a single register. + * NOTE: The memory and FIFO read/write registers cannot be accessed. + * @param[in] reg Register address. + * @param[out] data Register data. + * @return 0 if successful. + */ +int mpu_read_reg(unsigned char reg, unsigned char *data) +{ + if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) + return -1; + if (reg >= st.hw->num_reg) + return -1; + return i2c_read(st.hw->addr, reg, 1, data); +} + +/** + * @brief Initialize hardware. + * Initial configuration:\n + * Gyro FSR: +/- 2000DPS\n + * Accel FSR +/- 2G\n + * DLPF: 42Hz\n + * FIFO rate: 50Hz\n + * Clock source: Gyro PLL\n + * FIFO: Disabled.\n + * Data ready interrupt: Disabled, active low, unlatched. + * @param[in] int_param Platform-specific parameters to interrupt API. + * @return 0 if successful. + */ +int mpu_init(struct int_param_s *int_param) +{ + unsigned char data[6]; + + /* Reset device. */ + data[0] = BIT_RESET; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + delay_ms(100); + + /* Wake up chip. */ + data[0] = 0x00; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + + st.chip_cfg.accel_half = 0; + +#ifdef MPU6500 + /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the + * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. + */ + data[0] = BIT_FIFO_SIZE_1024 | 0x8; + if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) + return -1; +#endif + + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.bypass_mode = 0xFF; +#ifdef AK89xx_SECONDARY + st.chip_cfg.compass_sample_rate = 0xFFFF; +#endif + /* mpu_set_sensors always preserves this setting. */ + st.chip_cfg.clk_src = INV_CLK_PLL; + /* Handled in next call to mpu_set_bypass. */ + st.chip_cfg.active_low_int = 1; + st.chip_cfg.latched_int = 0; + st.chip_cfg.int_motion_only = 0; + st.chip_cfg.lp_accel_mode = 0; + memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); + st.chip_cfg.dmp_on = 0; + st.chip_cfg.dmp_loaded = 0; + st.chip_cfg.dmp_sample_rate = 0; + + if (mpu_set_gyro_fsr(2000)) + return -1; + if (mpu_set_accel_fsr(2)) + return -1; + if (mpu_set_lpf(42)) + return -1; + if (mpu_set_sample_rate(50)) + return -1; + if (mpu_configure_fifo(0)) + return -1; + +#ifndef EMPL_TARGET_STM32F4 + if (int_param) + reg_int_cb(int_param); +#endif + +#ifdef AK89xx_SECONDARY + setup_compass(); + if (mpu_set_compass_sample_rate(10)) + return -1; +#else + /* Already disabled by setup_compass. */ + if (mpu_set_bypass(0)) + return -1; +#endif + + mpu_set_sensors(0); + return 0; +} + +/** + * @brief Enter low-power accel-only mode. + * In low-power accel mode, the chip goes to sleep and only wakes up to sample + * the accelerometer at one of the following frequencies: + * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz + * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz + * \n If the requested rate is not one listed above, the device will be set to + * the next highest rate. Requesting a rate above the maximum supported + * frequency will result in an error. + * \n To select a fractional wake-up frequency, round down the value passed to + * @e rate. + * @param[in] rate Minimum sampling rate, or zero to disable LP + * accel mode. + * @return 0 if successful. + */ +int mpu_lp_accel_mode(unsigned short rate) +{ + unsigned char tmp[2]; + + if (rate > 40) + return -1; + + if (!rate) { + mpu_set_int_latched(0); + tmp[0] = 0; + tmp[1] = BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) + return -1; + st.chip_cfg.lp_accel_mode = 0; + return 0; + } + /* For LP accel, we automatically configure the hardware to produce latched + * interrupts. In LP accel mode, the hardware cycles into sleep mode before + * it gets a chance to deassert the interrupt pin; therefore, we shift this + * responsibility over to the MCU. + * + * Any register read will clear the interrupt. + */ + mpu_set_int_latched(1); +#if defined MPU6050 + tmp[0] = BIT_LPA_CYCLE; + if (rate == 1) { + tmp[1] = INV_LPA_1_25HZ; + mpu_set_lpf(5); + } else if (rate <= 5) { + tmp[1] = INV_LPA_5HZ; + mpu_set_lpf(5); + } else if (rate <= 20) { + tmp[1] = INV_LPA_20HZ; + mpu_set_lpf(10); + } else { + tmp[1] = INV_LPA_40HZ; + mpu_set_lpf(20); + } + tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) + return -1; +#elif defined MPU6500 + /* Set wake frequency. */ + if (rate == 1) + tmp[0] = INV_LPA_1_25HZ; + else if (rate == 2) + tmp[0] = INV_LPA_2_5HZ; + else if (rate <= 5) + tmp[0] = INV_LPA_5HZ; + else if (rate <= 10) + tmp[0] = INV_LPA_10HZ; + else if (rate <= 20) + tmp[0] = INV_LPA_20HZ; + else if (rate <= 40) + tmp[0] = INV_LPA_40HZ; + else if (rate <= 80) + tmp[0] = INV_LPA_80HZ; + else if (rate <= 160) + tmp[0] = INV_LPA_160HZ; + else if (rate <= 320) + tmp[0] = INV_LPA_320HZ; + else + tmp[0] = INV_LPA_640HZ; + if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) + return -1; + tmp[0] = BIT_LPA_CYCLE; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) + return -1; +#endif + st.chip_cfg.sensors = INV_XYZ_ACCEL; + st.chip_cfg.clk_src = 0; + st.chip_cfg.lp_accel_mode = 1; + mpu_configure_fifo(0); + + return 0; +} + +/** + * @brief Read raw gyro data directly from the registers. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_gyro_reg(short *data, unsigned long *timestamp) +{ + unsigned char tmp[6]; + + if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) + return -1; + data[0] = (tmp[0] << 8) | tmp[1]; + data[1] = (tmp[2] << 8) | tmp[3]; + data[2] = (tmp[4] << 8) | tmp[5]; + if (timestamp) + get_ms(timestamp); + return 0; +} + +/** + * @brief Read raw accel data directly from the registers. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_accel_reg(short *data, unsigned long *timestamp) +{ + unsigned char tmp[6]; + + if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) + return -1; + data[0] = (tmp[0] << 8) | tmp[1]; + data[1] = (tmp[2] << 8) | tmp[3]; + data[2] = (tmp[4] << 8) | tmp[5]; + if (timestamp) + get_ms(timestamp); + return 0; +} + +/** + * @brief Read temperature data directly from the registers. + * @param[out] data Data in q16 format. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_temperature(long *data, unsigned long *timestamp) +{ + unsigned char tmp[2]; + short raw; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp)) + return -1; + raw = (tmp[0] << 8) | tmp[1]; + if (timestamp) + get_ms(timestamp); + + data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); + return 0; +} + +/** + * @brief Read biases to the accel bias 6500 registers. + * This function reads from the MPU6500 accel offset cancellations registers. + * The format are G in +-8G format. The register is initialized with OTP + * factory trim values. + * @param[in] accel_bias returned structure with the accel bias + * @return 0 if successful. + */ +int mpu_read_6500_accel_bias(long *accel_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x77, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x7A, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x7D, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +/** + * @brief Read biases to the accel bias 6050 registers. + * This function reads from the MPU6050 accel offset cancellations registers. + * The format are G in +-8G format. The register is initialized with OTP + * factory trim values. + * @param[in] accel_bias returned structure with the accel bias + * @return 0 if successful. + */ +int mpu_read_6050_accel_bias(long *accel_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x06, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x08, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x0A, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +int mpu_read_6500_gyro_bias(long *gyro_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x13, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x15, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x17, 2, &data[4])) + return -1; + gyro_bias[0] = ((long)data[0]<<8) | data[1]; + gyro_bias[1] = ((long)data[2]<<8) | data[3]; + gyro_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +/** + * @brief Push biases to the gyro bias 6500/6050 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-1000dps format. + * @param[in] gyro_bias New biases. + * @return 0 if successful. + */ +int mpu_set_gyro_bias_reg(long *gyro_bias) +{ + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + int i=0; + for(i=0;i<3;i++) { + gyro_bias[i]= (-gyro_bias[i]); + } + data[0] = (gyro_bias[0] >> 8) & 0xff; + data[1] = (gyro_bias[0]) & 0xff; + data[2] = (gyro_bias[1] >> 8) & 0xff; + data[3] = (gyro_bias[1]) & 0xff; + data[4] = (gyro_bias[2] >> 8) & 0xff; + data[5] = (gyro_bias[2]) & 0xff; + if (i2c_write(st.hw->addr, 0x13, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x15, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x17, 2, &data[4])) + return -1; + return 0; +} + +/** + * @brief Push biases to the accel bias 6050 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-8G format. + * @param[in] accel_bias New biases. + * @return 0 if successful. + */ +int mpu_set_accel_bias_6050_reg(const long *accel_bias) { + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + long accel_reg_bias[3] = {0, 0, 0}; + + if(mpu_read_6050_accel_bias(accel_reg_bias)) + return -1; + + accel_reg_bias[0] -= (accel_bias[0] & ~1); + accel_reg_bias[1] -= (accel_bias[1] & ~1); + accel_reg_bias[2] -= (accel_bias[2] & ~1); + + data[0] = (accel_reg_bias[0] >> 8) & 0xff; + data[1] = (accel_reg_bias[0]) & 0xff; + data[2] = (accel_reg_bias[1] >> 8) & 0xff; + data[3] = (accel_reg_bias[1]) & 0xff; + data[4] = (accel_reg_bias[2] >> 8) & 0xff; + data[5] = (accel_reg_bias[2]) & 0xff; + + if (i2c_write(st.hw->addr, 0x06, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x08, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x0A, 2, &data[4])) + return -1; + + return 0; +} + + + +/** + * @brief Push biases to the accel bias 6500 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-8G format. + * @param[in] accel_bias New biases. + * @return 0 if successful. + */ +int mpu_set_accel_bias_6500_reg(const long *accel_bias) { + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + long accel_reg_bias[3] = {0, 0, 0}; + + if(mpu_read_6500_accel_bias(accel_reg_bias)) + return -1; + + // Preserve bit 0 of factory value (for temperature compensation) + accel_reg_bias[0] -= (accel_bias[0] & ~1); + accel_reg_bias[1] -= (accel_bias[1] & ~1); + accel_reg_bias[2] -= (accel_bias[2] & ~1); + + data[0] = (accel_reg_bias[0] >> 8) & 0xff; + data[1] = (accel_reg_bias[0]) & 0xff; + data[2] = (accel_reg_bias[1] >> 8) & 0xff; + data[3] = (accel_reg_bias[1]) & 0xff; + data[4] = (accel_reg_bias[2] >> 8) & 0xff; + data[5] = (accel_reg_bias[2]) & 0xff; + + if (i2c_write(st.hw->addr, 0x77, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x7A, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x7D, 2, &data[4])) + return -1; + + return 0; +} + + +/** + * @brief Reset FIFO read/write pointers. + * @return 0 if successful. + */ +int mpu_reset_fifo(void) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + + if (st.chip_cfg.dmp_on) { + data = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + delay_ms(50); + data = BIT_DMP_EN | BIT_FIFO_EN; + if (st.chip_cfg.sensors & INV_XYZ_COMPASS) + data |= BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + if (st.chip_cfg.int_enable) + data = BIT_DMP_INT_EN; + else + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + data = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) + return -1; + } else { + data = BIT_FIFO_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) + data = BIT_FIFO_EN; + else + data = BIT_FIFO_EN | BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + delay_ms(50); + if (st.chip_cfg.int_enable) + data = BIT_DATA_RDY_EN; + else + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) + return -1; + } + return 0; +} + +/** + * @brief Get the gyro full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_gyro_fsr(unsigned short *fsr) +{ + switch (st.chip_cfg.gyro_fsr) { + case INV_FSR_250DPS: + fsr[0] = 250; + break; + case INV_FSR_500DPS: + fsr[0] = 500; + break; + case INV_FSR_1000DPS: + fsr[0] = 1000; + break; + case INV_FSR_2000DPS: + fsr[0] = 2000; + break; + default: + fsr[0] = 0; + break; + } + return 0; +} + +/** + * @brief Set the gyro full-scale range. + * @param[in] fsr Desired full-scale range. + * @return 0 if successful. + */ +int mpu_set_gyro_fsr(unsigned short fsr) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + switch (fsr) { + case 250: + data = INV_FSR_250DPS << 3; + break; + case 500: + data = INV_FSR_500DPS << 3; + break; + case 1000: + data = INV_FSR_1000DPS << 3; + break; + case 2000: + data = INV_FSR_2000DPS << 3; + break; + default: + return -1; + } + + if (st.chip_cfg.gyro_fsr == (data >> 3)) + return 0; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) + return -1; + st.chip_cfg.gyro_fsr = data >> 3; + return 0; +} + +/** + * @brief Get the accel full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_accel_fsr(unsigned char *fsr) +{ + switch (st.chip_cfg.accel_fsr) { + case INV_FSR_2G: + fsr[0] = 2; + break; + case INV_FSR_4G: + fsr[0] = 4; + break; + case INV_FSR_8G: + fsr[0] = 8; + break; + case INV_FSR_16G: + fsr[0] = 16; + break; + default: + return -1; + } + if (st.chip_cfg.accel_half) + fsr[0] <<= 1; + return 0; +} + +/** + * @brief Set the accel full-scale range. + * @param[in] fsr Desired full-scale range. + * @return 0 if successful. + */ +int mpu_set_accel_fsr(unsigned char fsr) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + switch (fsr) { + case 2: + data = INV_FSR_2G << 3; + break; + case 4: + data = INV_FSR_4G << 3; + break; + case 8: + data = INV_FSR_8G << 3; + break; + case 16: + data = INV_FSR_16G << 3; + break; + default: + return -1; + } + + if (st.chip_cfg.accel_fsr == (data >> 3)) + return 0; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) + return -1; + st.chip_cfg.accel_fsr = data >> 3; + return 0; +} + +/** + * @brief Get the current DLPF setting. + * @param[out] lpf Current LPF setting. + * 0 if successful. + */ +int mpu_get_lpf(unsigned short *lpf) +{ + switch (st.chip_cfg.lpf) { + case INV_FILTER_188HZ: + lpf[0] = 188; + break; + case INV_FILTER_98HZ: + lpf[0] = 98; + break; + case INV_FILTER_42HZ: + lpf[0] = 42; + break; + case INV_FILTER_20HZ: + lpf[0] = 20; + break; + case INV_FILTER_10HZ: + lpf[0] = 10; + break; + case INV_FILTER_5HZ: + lpf[0] = 5; + break; + case INV_FILTER_256HZ_NOLPF2: + case INV_FILTER_2100HZ_NOLPF: + default: + lpf[0] = 0; + break; + } + return 0; +} + +/** + * @brief Set digital low pass filter. + * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. + * @param[in] lpf Desired LPF setting. + * @return 0 if successful. + */ +int mpu_set_lpf(unsigned short lpf) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (lpf >= 188) + data = INV_FILTER_188HZ; + else if (lpf >= 98) + data = INV_FILTER_98HZ; + else if (lpf >= 42) + data = INV_FILTER_42HZ; + else if (lpf >= 20) + data = INV_FILTER_20HZ; + else if (lpf >= 10) + data = INV_FILTER_10HZ; + else + data = INV_FILTER_5HZ; + + if (st.chip_cfg.lpf == data) + return 0; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data)) + return -1; + st.chip_cfg.lpf = data; + return 0; +} + +/** + * @brief Get sampling rate. + * @param[out] rate Current sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_get_sample_rate(unsigned short *rate) +{ + if (st.chip_cfg.dmp_on) + return -1; + else + rate[0] = st.chip_cfg.sample_rate; + return 0; +} + +/** + * @brief Set sampling rate. + * Sampling rate must be between 4Hz and 1kHz. + * @param[in] rate Desired sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_set_sample_rate(unsigned short rate) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (st.chip_cfg.dmp_on) + return -1; + else { + if (st.chip_cfg.lp_accel_mode) { + if (rate && (rate <= 40)) { + /* Just stay in low-power accel mode. */ + mpu_lp_accel_mode(rate); + return 0; + } + /* Requested rate exceeds the allowed frequencies in LP accel mode, + * switch back to full-power mode. + */ + mpu_lp_accel_mode(0); + } + if (rate < 4) + rate = 4; + else if (rate > 1000) + rate = 1000; + + data = 1000 / rate - 1; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data)) + return -1; + + st.chip_cfg.sample_rate = 1000 / (1 + data); + +#ifdef AK89xx_SECONDARY + mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); +#endif + + /* Automatically set LPF to 1/2 sampling rate. */ + mpu_set_lpf(st.chip_cfg.sample_rate >> 1); + return 0; + } +} + +/** + * @brief Get compass sampling rate. + * @param[out] rate Current compass sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_get_compass_sample_rate(unsigned short *rate) +{ +#ifdef AK89xx_SECONDARY + rate[0] = st.chip_cfg.compass_sample_rate; + return 0; +#else + rate[0] = 0; + return -1; +#endif +} + +/** + * @brief Set compass sampling rate. + * The compass on the auxiliary I2C bus is read by the MPU hardware at a + * maximum of 100Hz. The actual rate can be set to a fraction of the gyro + * sampling rate. + * + * \n WARNING: The new rate may be different than what was requested. Call + * mpu_get_compass_sample_rate to check the actual setting. + * @param[in] rate Desired compass sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_set_compass_sample_rate(unsigned short rate) +{ +#ifdef AK89xx_SECONDARY + unsigned char div; + if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) + return -1; + + div = st.chip_cfg.sample_rate / rate - 1; + if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) + return -1; + st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); + return 0; +#else + return -1; +#endif +} + +/** + * @brief Get gyro sensitivity scale factor. + * @param[out] sens Conversion from hardware units to dps. + * @return 0 if successful. + */ +int mpu_get_gyro_sens(float *sens) +{ + switch (st.chip_cfg.gyro_fsr) { + case INV_FSR_250DPS: + sens[0] = 131.f; + break; + case INV_FSR_500DPS: + sens[0] = 65.5f; + break; + case INV_FSR_1000DPS: + sens[0] = 32.8f; + break; + case INV_FSR_2000DPS: + sens[0] = 16.4f; + break; + default: + return -1; + } + return 0; +} + +/** + * @brief Get accel sensitivity scale factor. + * @param[out] sens Conversion from hardware units to g's. + * @return 0 if successful. + */ +int mpu_get_accel_sens(unsigned short *sens) +{ + switch (st.chip_cfg.accel_fsr) { + case INV_FSR_2G: + sens[0] = 16384; + break; + case INV_FSR_4G: + sens[0] = 8192; + break; + case INV_FSR_8G: + sens[0] = 4096; + break; + case INV_FSR_16G: + sens[0] = 2048; + break; + default: + return -1; + } + if (st.chip_cfg.accel_half) + sens[0] >>= 1; + return 0; +} + +/** + * @brief Get current FIFO configuration. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * @param[out] sensors Mask of sensors in FIFO. + * @return 0 if successful. + */ +int mpu_get_fifo_config(unsigned char *sensors) +{ + sensors[0] = st.chip_cfg.fifo_enable; + return 0; +} + +/** + * @brief Select which sensors are pushed to FIFO. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * @param[in] sensors Mask of sensors to push to FIFO. + * @return 0 if successful. + */ +int mpu_configure_fifo(unsigned char sensors) +{ + unsigned char prev; + int result = 0; + + /* Compass data isn't going into the FIFO. Stop trying. */ + sensors &= ~INV_XYZ_COMPASS; + + if (st.chip_cfg.dmp_on) + return 0; + else { + if (!(st.chip_cfg.sensors)) + return -1; + prev = st.chip_cfg.fifo_enable; + st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; + if (st.chip_cfg.fifo_enable != sensors) + /* You're not getting what you asked for. Some sensors are + * asleep. + */ + result = -1; + else + result = 0; + if (sensors || st.chip_cfg.lp_accel_mode) + set_int_enable(1); + else + set_int_enable(0); + if (sensors) { + if (mpu_reset_fifo()) { + st.chip_cfg.fifo_enable = prev; + return -1; + } + } + } + + return result; +} + +/** + * @brief Get current power state. + * @param[in] power_on 1 if turned on, 0 if suspended. + * @return 0 if successful. + */ +int mpu_get_power_state(unsigned char *power_on) +{ + if (st.chip_cfg.sensors) + power_on[0] = 1; + else + power_on[0] = 0; + return 0; +} + +/** + * @brief Turn specific sensors on/off. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n INV_XYZ_COMPASS + * @param[in] sensors Mask of sensors to wake. + * @return 0 if successful. + */ +int mpu_set_sensors(unsigned char sensors) +{ + unsigned char data; +#ifdef AK89xx_SECONDARY + unsigned char user_ctrl; +#endif + + if (sensors & INV_XYZ_GYRO) + data = INV_CLK_PLL; + else if (sensors) + data = 0; + else + data = BIT_SLEEP; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { + st.chip_cfg.sensors = 0; + return -1; + } + st.chip_cfg.clk_src = data & ~BIT_SLEEP; + + data = 0; + if (!(sensors & INV_X_GYRO)) + data |= BIT_STBY_XG; + if (!(sensors & INV_Y_GYRO)) + data |= BIT_STBY_YG; + if (!(sensors & INV_Z_GYRO)) + data |= BIT_STBY_ZG; + if (!(sensors & INV_XYZ_ACCEL)) + data |= BIT_STBY_XYZA; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { + st.chip_cfg.sensors = 0; + return -1; + } + + if (sensors && (sensors != INV_XYZ_ACCEL)) + /* Latched interrupts only used in LP accel mode. */ + mpu_set_int_latched(0); + +#ifdef AK89xx_SECONDARY +#ifdef AK89xx_BYPASS + if (sensors & INV_XYZ_COMPASS) + mpu_set_bypass(1); + else + mpu_set_bypass(0); +#else + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) + return -1; + /* Handle AKM power management. */ + if (sensors & INV_XYZ_COMPASS) { + data = AKM_SINGLE_MEASUREMENT; + user_ctrl |= BIT_AUX_IF_EN; + } else { + data = AKM_POWER_DOWN; + user_ctrl &= ~BIT_AUX_IF_EN; + } + if (st.chip_cfg.dmp_on) + user_ctrl |= BIT_DMP_EN; + else + user_ctrl &= ~BIT_DMP_EN; + if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) + return -1; + /* Enable/disable I2C master mode. */ + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) + return -1; +#endif +#endif + + st.chip_cfg.sensors = sensors; + st.chip_cfg.lp_accel_mode = 0; + delay_ms(50); + return 0; +} + +/** + * @brief Read the MPU interrupt status registers. + * @param[out] status Mask of interrupt bits. + * @return 0 if successful. + */ +int mpu_get_int_status(short *status) +{ + unsigned char tmp[2]; + if (!st.chip_cfg.sensors) + return -1; + if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) + return -1; + status[0] = (tmp[0] << 8) | tmp[1]; + return 0; +} + +/** + * @brief Get one packet from the FIFO. + * If @e sensors does not contain a particular sensor, disregard the data + * returned to that pointer. + * \n @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n If the FIFO has no new data, @e sensors will be zero. + * \n If the FIFO is disabled, @e sensors will be zero and this function will + * return a non-zero error code. + * @param[out] gyro Gyro data in hardware units. + * @param[out] accel Accel data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. + * @param[out] sensors Mask of sensors read from FIFO. + * @param[out] more Number of remaining packets. + * @return 0 if successful. + */ +int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, + unsigned char *sensors, unsigned char *more) +{ + /* Assumes maximum packet size is gyro (6) + accel (6). */ + unsigned char data[MAX_PACKET_LENGTH]; + unsigned char packet_size = 0; + unsigned short fifo_count, index = 0; + + if (st.chip_cfg.dmp_on) + return -1; + + sensors[0] = 0; + if (!st.chip_cfg.sensors) + return -1; + if (!st.chip_cfg.fifo_enable) + return -1; + + if (st.chip_cfg.fifo_enable & INV_X_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_Y_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_Z_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) + packet_size += 6; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + fifo_count = (data[0] << 8) | data[1]; + if (fifo_count < packet_size) + return 0; +// log_i("FIFO count: %hd\n", fifo_count); + if (fifo_count > (st.hw->max_fifo >> 1)) { + /* FIFO is 50% full, better check overflow bit. */ + if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) + return -1; + if (data[0] & BIT_FIFO_OVERFLOW) { + mpu_reset_fifo(); + return -2; + } + } + get_ms((unsigned long*)timestamp); + + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) + return -1; + more[0] = fifo_count / packet_size - 1; + sensors[0] = 0; + + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { + accel[0] = (data[index+0] << 8) | data[index+1]; + accel[1] = (data[index+2] << 8) | data[index+3]; + accel[2] = (data[index+4] << 8) | data[index+5]; + sensors[0] |= INV_XYZ_ACCEL; + index += 6; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { + gyro[0] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_X_GYRO; + index += 2; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { + gyro[1] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_Y_GYRO; + index += 2; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { + gyro[2] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_Z_GYRO; + index += 2; + } + + return 0; +} + +/** + * @brief Get one unparsed packet from the FIFO. + * This function should be used if the packet is to be parsed elsewhere. + * @param[in] length Length of one FIFO packet. + * @param[in] data FIFO packet. + * @param[in] more Number of remaining packets. + */ +int mpu_read_fifo_stream(unsigned short length, unsigned char *data, + unsigned char *more) +{ + unsigned char tmp[2]; + unsigned short fifo_count; + if (!st.chip_cfg.dmp_on) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) + return -1; + fifo_count = (tmp[0] << 8) | tmp[1]; + if (fifo_count < length) { + more[0] = 0; + return -1; + } + if (fifo_count > (st.hw->max_fifo >> 1)) { + /* FIFO is 50% full, better check overflow bit. */ + if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp)) + return -1; + if (tmp[0] & BIT_FIFO_OVERFLOW) { + mpu_reset_fifo(); + return -2; + } + } + + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data)) + return -1; + more[0] = fifo_count / length - 1; + return 0; +} + +/** + * @brief Set device to bypass mode. + * @param[in] bypass_on 1 to enable bypass mode. + * @return 0 if successful. + */ +int mpu_set_bypass(unsigned char bypass_on) +{ + unsigned char tmp; + + if (st.chip_cfg.bypass_mode == bypass_on) + return 0; + + if (bypass_on) { + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + tmp &= ~BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + delay_ms(3); + tmp = BIT_BYPASS_EN; + if (st.chip_cfg.active_low_int) + tmp |= BIT_ACTL; + if (st.chip_cfg.latched_int) + tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + } else { + /* Enable I2C master mode if compass is being used. */ + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + if (st.chip_cfg.sensors & INV_XYZ_COMPASS) + tmp |= BIT_AUX_IF_EN; + else + tmp &= ~BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + delay_ms(3); + if (st.chip_cfg.active_low_int) + tmp = BIT_ACTL; + else + tmp = 0; + if (st.chip_cfg.latched_int) + tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + } + st.chip_cfg.bypass_mode = bypass_on; + return 0; +} + +/** + * @brief Set interrupt level. + * @param[in] active_low 1 for active low, 0 for active high. + * @return 0 if successful. + */ +int mpu_set_int_level(unsigned char active_low) +{ + st.chip_cfg.active_low_int = active_low; + return 0; +} + +/** + * @brief Enable latched interrupts. + * Any MPU register will clear the interrupt. + * @param[in] enable 1 to enable, 0 to disable. + * @return 0 if successful. + */ +int mpu_set_int_latched(unsigned char enable) +{ + unsigned char tmp; + if (st.chip_cfg.latched_int == enable) + return 0; + + if (enable) + tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; + else + tmp = 0; + if (st.chip_cfg.bypass_mode) + tmp |= BIT_BYPASS_EN; + if (st.chip_cfg.active_low_int) + tmp |= BIT_ACTL; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + st.chip_cfg.latched_int = enable; + return 0; +} + +#ifdef MPU6050 +static int get_accel_prod_shift(float *st_shift) +{ + unsigned char tmp[4], shift_code[3], ii; + + if (i2c_read(st.hw->addr, 0x0D, 4, tmp)) + return 0x07; + + shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); + shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); + shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); + for (ii = 0; ii < 3; ii++) { + if (!shift_code[ii]) { + st_shift[ii] = 0.f; + continue; + } + /* Equivalent to.. + * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) + */ + st_shift[ii] = 0.34f; + while (--shift_code[ii]) + st_shift[ii] *= 1.034f; + } + return 0; +} + +static int accel_self_test(long *bias_regular, long *bias_st) +{ + int jj, result = 0; + float st_shift[3], st_shift_cust, st_shift_var; + + get_accel_prod_shift(st_shift); + for(jj = 0; jj < 3; jj++) { + st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; + if (st_shift[jj]) { + st_shift_var = st_shift_cust / st_shift[jj] - 1.f; + if (fabs(st_shift_var) > test.max_accel_var) + result |= 1 << jj; + } else if ((st_shift_cust < test.min_g) || + (st_shift_cust > test.max_g)) + result |= 1 << jj; + } + + return result; +} + +static int gyro_self_test(long *bias_regular, long *bias_st) +{ + int jj, result = 0; + unsigned char tmp[3]; + float st_shift, st_shift_cust, st_shift_var; + + if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) + return 0x07; + + tmp[0] &= 0x1F; + tmp[1] &= 0x1F; + tmp[2] &= 0x1F; + + for (jj = 0; jj < 3; jj++) { + st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; + if (tmp[jj]) { + st_shift = 3275.f / test.gyro_sens; + while (--tmp[jj]) + st_shift *= 1.046f; + st_shift_var = st_shift_cust / st_shift - 1.f; + if (fabs(st_shift_var) > test.max_gyro_var) + result |= 1 << jj; + } else if ((st_shift_cust < test.min_dps) || + (st_shift_cust > test.max_dps)) + result |= 1 << jj; + } + return result; +} + +#endif +#ifdef AK89xx_SECONDARY +static int compass_self_test(void) +{ + unsigned char tmp[6]; + unsigned char tries = 10; + int result = 0x07; + short data; + + mpu_set_bypass(1); + + tmp[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) + return 0x07; + tmp[0] = AKM_BIT_SELF_TEST; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) + goto AKM_restore; + tmp[0] = AKM_MODE_SELF_TEST; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) + goto AKM_restore; + + do { + delay_ms(10); + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) + goto AKM_restore; + if (tmp[0] & AKM_DATA_READY) + break; + } while (tries--); + if (!(tmp[0] & AKM_DATA_READY)) + goto AKM_restore; + + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) + goto AKM_restore; + + result = 0; +#if defined MPU9150 + data = (short)(tmp[1] << 8) | tmp[0]; + if ((data > 100) || (data < -100)) + result |= 0x01; + data = (short)(tmp[3] << 8) | tmp[2]; + if ((data > 100) || (data < -100)) + result |= 0x02; + data = (short)(tmp[5] << 8) | tmp[4]; + if ((data > -300) || (data < -1000)) + result |= 0x04; +#elif defined MPU9250 + data = (short)(tmp[1] << 8) | tmp[0]; + if ((data > 200) || (data < -200)) + result |= 0x01; + data = (short)(tmp[3] << 8) | tmp[2]; + if ((data > 200) || (data < -200)) + result |= 0x02; + data = (short)(tmp[5] << 8) | tmp[4]; + if ((data > -800) || (data < -3200)) + result |= 0x04; +#endif +AKM_restore: + tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; + i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); + tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; + i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); + mpu_set_bypass(0); + return result; +} +#endif + +static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) +{ + unsigned char data[MAX_PACKET_LENGTH]; + unsigned char packet_count, ii; + unsigned short fifo_count; + + data[0] = 0x01; + data[1] = 0; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) + return -1; + delay_ms(200); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + delay_ms(15); + data[0] = st.test->reg_lpf; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) + return -1; + data[0] = st.test->reg_rate_div; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) + return -1; + if (hw_test) + data[0] = st.test->reg_gyro_fsr | 0xE0; + else + data[0] = st.test->reg_gyro_fsr; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) + return -1; + + if (hw_test) + data[0] = st.test->reg_accel_fsr | 0xE0; + else + data[0] = test.reg_accel_fsr; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) + return -1; + if (hw_test) + delay_ms(200); + + /* Fill FIFO for test.wait_ms milliseconds. */ + data[0] = BIT_FIFO_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + + data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + delay_ms(test.wait_ms); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + + fifo_count = (data[0] << 8) | data[1]; + packet_count = fifo_count / MAX_PACKET_LENGTH; + gyro[0] = gyro[1] = gyro[2] = 0; + accel[0] = accel[1] = accel[2] = 0; + + for (ii = 0; ii < packet_count; ii++) { + short accel_cur[3], gyro_cur[3]; + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) + return -1; + accel_cur[0] = ((short)data[0] << 8) | data[1]; + accel_cur[1] = ((short)data[2] << 8) | data[3]; + accel_cur[2] = ((short)data[4] << 8) | data[5]; + accel[0] += (long)accel_cur[0]; + accel[1] += (long)accel_cur[1]; + accel[2] += (long)accel_cur[2]; + gyro_cur[0] = (((short)data[6] << 8) | data[7]); + gyro_cur[1] = (((short)data[8] << 8) | data[9]); + gyro_cur[2] = (((short)data[10] << 8) | data[11]); + gyro[0] += (long)gyro_cur[0]; + gyro[1] += (long)gyro_cur[1]; + gyro[2] += (long)gyro_cur[2]; + } +#ifdef EMPL_NO_64BIT + gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); + gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); + gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); + if (has_accel) { + accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / + packet_count); + accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / + packet_count); + accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / + packet_count); + /* Don't remove gravity! */ + accel[2] -= 65536L; + } +#else + gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); + gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); + gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); + accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / + packet_count); + accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / + packet_count); + accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / + packet_count); + /* Don't remove gravity! */ + if (accel[2] > 0L) + accel[2] -= 65536L; + else + accel[2] += 65536L; +#endif + + return 0; +} + +#ifdef MPU6500 +#define REG_6500_XG_ST_DATA 0x0 +#define REG_6500_XA_ST_DATA 0xD +static const unsigned short mpu_6500_st_tb[256] = { + 2620,2646,2672,2699,2726,2753,2781,2808, //7 + 2837,2865,2894,2923,2952,2981,3011,3041, //15 + 3072,3102,3133,3165,3196,3228,3261,3293, //23 + 3326,3359,3393,3427,3461,3496,3531,3566, //31 + 3602,3638,3674,3711,3748,3786,3823,3862, //39 + 3900,3939,3979,4019,4059,4099,4140,4182, //47 + 4224,4266,4308,4352,4395,4439,4483,4528, //55 + 4574,4619,4665,4712,4759,4807,4855,4903, //63 + 4953,5002,5052,5103,5154,5205,5257,5310, //71 + 5363,5417,5471,5525,5581,5636,5693,5750, //79 + 5807,5865,5924,5983,6043,6104,6165,6226, //87 + 6289,6351,6415,6479,6544,6609,6675,6742, //95 + 6810,6878,6946,7016,7086,7157,7229,7301, //103 + 7374,7448,7522,7597,7673,7750,7828,7906, //111 + 7985,8065,8145,8227,8309,8392,8476,8561, //119 + 8647,8733,8820,8909,8998,9088,9178,9270, + 9363,9457,9551,9647,9743,9841,9939,10038, + 10139,10240,10343,10446,10550,10656,10763,10870, + 10979,11089,11200,11312,11425,11539,11654,11771, + 11889,12008,12128,12249,12371,12495,12620,12746, + 12874,13002,13132,13264,13396,13530,13666,13802, + 13940,14080,14221,14363,14506,14652,14798,14946, + 15096,15247,15399,15553,15709,15866,16024,16184, + 16346,16510,16675,16842,17010,17180,17352,17526, + 17701,17878,18057,18237,18420,18604,18790,18978, + 19167,19359,19553,19748,19946,20145,20347,20550, + 20756,20963,21173,21385,21598,21814,22033,22253, + 22475,22700,22927,23156,23388,23622,23858,24097, + 24338,24581,24827,25075,25326,25579,25835,26093, + 26354,26618,26884,27153,27424,27699,27976,28255, + 28538,28823,29112,29403,29697,29994,30294,30597, + 30903,31212,31524,31839,32157,32479,32804,33132 +}; +static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug) +{ + int i, result = 0, otp_value_zero = 0; + float accel_st_al_min, accel_st_al_max; + float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max; + unsigned char regs[3]; + if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) { + if(debug) + log_i("Reading OTP Register Error.\n"); + return 0x07; + } + if(debug) + log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]); + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.accel_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + if(otp_value_zero == 0) { + if(debug) + log_i("ACCEL:CRITERIA A\n"); + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + if(debug) { + log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", + st_shift_cust[i]/1.f, bias_regular[i]/1.f, + bias_st[i]/1.f); + log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); + } + + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f; + + if(debug) + log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, + test.max_accel_var/1.f); + + if (fabs(st_shift_ratio[i]) > test.max_accel_var) { + if(debug) + log_i("ACCEL Fail Axis = %d\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + accel_st_al_min = test.min_g * 65536.f; + accel_st_al_max = test.max_g * 65536.f; + + if(debug) { + log_i("ACCEL:CRITERIA B\r\n"); + log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f); + log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f); + } + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) + log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f); + if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) { + if(debug) + log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i); + result |= 1 << i; //Error condition + } + } + } + + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + accel_offset_max = test.max_g_offset * 65536.f; + if(debug) + log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > accel_offset_max) { + if(debug) + log_i("FAILED: Accel axis:%d = %ld > 500mg\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } + + return result; +} + +static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug) +{ + int i, result = 0, otp_value_zero = 0; + float gyro_st_al_max; + float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max; + unsigned char regs[3]; + + if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) { + if(debug) + log_i("Reading OTP Register Error.\n"); + return 0x07; + } + + if(debug) + log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.gyro_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + + if(otp_value_zero == 0) { + if(debug) + log_i("GYRO:CRITERIA A\n"); + /* Self Test Pass/Fail Criteria A */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) { + log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", + st_shift_cust[i]/1.f, bias_regular[i]/1.f, + bias_st[i]/1.f); + log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); + } + + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i]; + + if(debug) + log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, + test.max_gyro_var/1.f); + + if (fabs(st_shift_ratio[i]) < test.max_gyro_var) { + if(debug) + log_i("Gyro Fail Axis = %d\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + gyro_st_al_max = test.max_dps * 65536.f; + + if(debug) { + log_i("GYRO:CRITERIA B\r\n"); + log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f); + } + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) + log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f); + if(st_shift_cust[i] < gyro_st_al_max) { + if(debug) + log_i("GYRO FAIL axis:%d greater than 60dps\n", i); + result |= 1 << i; //Error condition + } + } + } + + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + gyro_offset_max = test.min_dps * 65536.f; + if(debug) + log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > gyro_offset_max) { + if(debug) + log_i("FAILED: Gyro axis:%d = %ld > 20dps\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } + return result; +} + +static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug) +{ + unsigned char data[HWST_MAX_PACKET_LENGTH]; + unsigned char packet_count, ii; + unsigned short fifo_count; + int s = 0, read_size = 0, ind; + + data[0] = 0x01; + data[1] = 0; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) + return -1; + delay_ms(200); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + delay_ms(15); + data[0] = st.test->reg_lpf; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) + return -1; + data[0] = st.test->reg_rate_div; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) + return -1; + if (hw_test) + data[0] = st.test->reg_gyro_fsr | 0xE0; + else + data[0] = st.test->reg_gyro_fsr; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) + return -1; + + if (hw_test) + data[0] = st.test->reg_accel_fsr | 0xE0; + else + data[0] = test.reg_accel_fsr; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) + return -1; + + delay_ms(test.wait_ms); //wait 200ms for sensors to stabilize + + /* Enable FIFO */ + data[0] = BIT_FIFO_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + //initialize the bias return values + gyro[0] = gyro[1] = gyro[2] = 0; + accel[0] = accel[1] = accel[2] = 0; + + if(debug) + log_i("Starting Bias Loop Reads\n"); + + //start reading samples + while (s < test.packet_thresh) { + delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + fifo_count = (data[0] << 8) | data[1]; + packet_count = fifo_count / MAX_PACKET_LENGTH; + if ((test.packet_thresh - s) < packet_count) + packet_count = test.packet_thresh - s; + read_size = packet_count * MAX_PACKET_LENGTH; + + //burst read from FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data)) + return -1; + ind = 0; + for (ii = 0; ii < packet_count; ii++) { + short accel_cur[3], gyro_cur[3]; + accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1]; + accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3]; + accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5]; + accel[0] += (long)accel_cur[0]; + accel[1] += (long)accel_cur[1]; + accel[2] += (long)accel_cur[2]; + gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]); + gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]); + gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]); + gyro[0] += (long)gyro_cur[0]; + gyro[1] += (long)gyro_cur[1]; + gyro[2] += (long)gyro_cur[2]; + ind += MAX_PACKET_LENGTH; + } + s += packet_count; + } + + if(debug) + log_i("Samples: %d\n", s); + + //stop FIFO + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s); + gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s); + gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s); + accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s); + accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s); + accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s); + /* remove gravity from bias calculation */ + if (accel[2] > 0L) + accel[2] -= 65536L; + else + accel[2] += 65536L; + + + if(debug) { + log_i("Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f); + log_i("Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f); + } + + return 0; +} +/** + * @brief Trigger gyro/accel/compass self-test for MPU6500/MPU9250 + * On success/error, the self-test returns a mask representing the sensor(s) + * that failed. For each bit, a one (1) represents a "pass" case; conversely, + * a zero (0) indicates a failure. + * + * \n The mask is defined as follows: + * \n Bit 0: Gyro. + * \n Bit 1: Accel. + * \n Bit 2: Compass. + * + * @param[out] gyro Gyro biases in q16 format. + * @param[out] accel Accel biases (if applicable) in q16 format. + * @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver. + * @return Result mask (see above). + */ +int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) +{ + const unsigned char tries = 2; + long gyro_st[3], accel_st[3]; + unsigned char accel_result, gyro_result; +#ifdef AK89xx_SECONDARY + unsigned char compass_result; +#endif + int ii; + + int result; + unsigned char accel_fsr, fifo_sensors, sensors_on; + unsigned short gyro_fsr, sample_rate, lpf; + unsigned char dmp_was_on; + + + + if(debug) + log_i("Starting MPU6500 HWST!\r\n"); + + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + dmp_was_on = 1; + } else + dmp_was_on = 0; + + /* Get initial settings. */ + mpu_get_gyro_fsr(&gyro_fsr); + mpu_get_accel_fsr(&accel_fsr); + mpu_get_lpf(&lpf); + mpu_get_sample_rate(&sample_rate); + sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&fifo_sensors); + + if(debug) + log_i("Retrieving Biases\r\n"); + + for (ii = 0; ii < tries; ii++) + if (!get_st_6500_biases(gyro, accel, 0, debug)) + break; + if (ii == tries) { + /* If we reach this point, we most likely encountered an I2C error. + * We'll just report an error for all three sensors. + */ + if(debug) + log_i("Retrieving Biases Error - possible I2C error\n"); + + result = 0; + goto restore; + } + + if(debug) + log_i("Retrieving ST Biases\n"); + + for (ii = 0; ii < tries; ii++) + if (!get_st_6500_biases(gyro_st, accel_st, 1, debug)) + break; + if (ii == tries) { + + if(debug) + log_i("Retrieving ST Biases Error - possible I2C error\n"); + + /* Again, probably an I2C error. */ + result = 0; + goto restore; + } + + accel_result = accel_6500_self_test(accel, accel_st, debug); + if(debug) + log_i("Accel Self Test Results: %d\n", accel_result); + + gyro_result = gyro_6500_self_test(gyro, gyro_st, debug); + if(debug) + log_i("Gyro Self Test Results: %d\n", gyro_result); + + result = 0; + if (!gyro_result) + result |= 0x01; + if (!accel_result) + result |= 0x02; + +#ifdef AK89xx_SECONDARY + compass_result = compass_self_test(); + if(debug) + log_i("Compass Self Test Results: %d\n", compass_result); + if (!compass_result) + result |= 0x04; +#else + result |= 0x04; +#endif +restore: + if(debug) + log_i("Exiting HWST\n"); + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_gyro_fsr(gyro_fsr); + mpu_set_accel_fsr(accel_fsr); + mpu_set_lpf(lpf); + mpu_set_sample_rate(sample_rate); + mpu_set_sensors(sensors_on); + mpu_configure_fifo(fifo_sensors); + + if (dmp_was_on) + mpu_set_dmp_state(1); + + return result; +} +#endif + /* + * \n This function must be called with the device either face-up or face-down + * (z-axis is parallel to gravity). + * @param[out] gyro Gyro biases in q16 format. + * @param[out] accel Accel biases (if applicable) in q16 format. + * @return Result mask (see above). + */ +int mpu_run_self_test(long *gyro, long *accel) +{ +#ifdef MPU6050 + const unsigned char tries = 2; + long gyro_st[3], accel_st[3]; + unsigned char accel_result, gyro_result; +#ifdef AK89xx_SECONDARY + unsigned char compass_result; +#endif + int ii; +#endif + int result; + unsigned char accel_fsr, fifo_sensors, sensors_on; + unsigned short gyro_fsr, sample_rate, lpf; + unsigned char dmp_was_on; + + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + dmp_was_on = 1; + } else + dmp_was_on = 0; + + /* Get initial settings. */ + mpu_get_gyro_fsr(&gyro_fsr); + mpu_get_accel_fsr(&accel_fsr); + mpu_get_lpf(&lpf); + mpu_get_sample_rate(&sample_rate); + sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&fifo_sensors); + + /* For older chips, the self-test will be different. */ +#if defined MPU6050 + for (ii = 0; ii < tries; ii++) + if (!get_st_biases(gyro, accel, 0)) + break; + if (ii == tries) { + /* If we reach this point, we most likely encountered an I2C error. + * We'll just report an error for all three sensors. + */ + result = 0; + goto restore; + } + for (ii = 0; ii < tries; ii++) + if (!get_st_biases(gyro_st, accel_st, 1)) + break; + if (ii == tries) { + /* Again, probably an I2C error. */ + result = 0; + goto restore; + } + accel_result = accel_self_test(accel, accel_st); + gyro_result = gyro_self_test(gyro, gyro_st); + + result = 0; + if (!gyro_result) + result |= 0x01; + if (!accel_result) + result |= 0x02; + +#ifdef AK89xx_SECONDARY + compass_result = compass_self_test(); + if (!compass_result) + result |= 0x04; +#else + result |= 0x04; +#endif +restore: +#elif defined MPU6500 + /* For now, this function will return a "pass" result for all three sensors + * for compatibility with current test applications. + */ + get_st_biases(gyro, accel, 0); + result = 0x7; +#endif + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_gyro_fsr(gyro_fsr); + mpu_set_accel_fsr(accel_fsr); + mpu_set_lpf(lpf); + mpu_set_sample_rate(sample_rate); + mpu_set_sensors(sensors_on); + mpu_configure_fifo(fifo_sensors); + + if (dmp_was_on) + mpu_set_dmp_state(1); + + return result; +} + +/** + * @brief Write to the DMP memory. + * This function prevents I2C writes past the bank boundaries. The DMP memory + * is only accessible when the chip is awake. + * @param[in] mem_addr Memory location (bank << 8 | start address) + * @param[in] length Number of bytes to write. + * @param[in] data Bytes to write to memory. + * @return 0 if successful. + */ +int mpu_write_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data) +{ + unsigned char tmp[2]; + + if (!data) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + tmp[0] = (unsigned char)(mem_addr >> 8); + tmp[1] = (unsigned char)(mem_addr & 0xFF); + + /* Check bank boundaries. */ + if (tmp[1] + length > st.hw->bank_size) + return -1; + + if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) + return -1; + if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data)) + return -1; + return 0; +} + +/** + * @brief Read from the DMP memory. + * This function prevents I2C reads past the bank boundaries. The DMP memory + * is only accessible when the chip is awake. + * @param[in] mem_addr Memory location (bank << 8 | start address) + * @param[in] length Number of bytes to read. + * @param[out] data Bytes read from memory. + * @return 0 if successful. + */ +int mpu_read_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data) +{ + unsigned char tmp[2]; + + if (!data) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + tmp[0] = (unsigned char)(mem_addr >> 8); + tmp[1] = (unsigned char)(mem_addr & 0xFF); + + /* Check bank boundaries. */ + if (tmp[1] + length > st.hw->bank_size) + return -1; + + if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) + return -1; + if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data)) + return -1; + return 0; +} + +/** + * @brief Load and verify DMP image. + * @param[in] length Length of DMP image. + * @param[in] firmware DMP code. + * @param[in] start_addr Starting address of DMP code memory. + * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. + * @return 0 if successful. + */ +int mpu_load_firmware(unsigned short length, const unsigned char *firmware, + unsigned short start_addr, unsigned short sample_rate) +{ + unsigned short ii; + unsigned short this_write; + /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ +#define LOAD_CHUNK (16) + unsigned char cur[LOAD_CHUNK], tmp[2]; + + if (st.chip_cfg.dmp_loaded) + /* DMP should only be loaded once. */ + return -1; + + if (!firmware) + return -1; + for (ii = 0; ii < length; ii += this_write) { + this_write = min(LOAD_CHUNK, length - ii); + if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) + return -1; + if (mpu_read_mem(ii, this_write, cur)) + return -1; + if (memcmp(firmware+ii, cur, this_write)) + return -2; + } + + /* Set program start address. */ + tmp[0] = start_addr >> 8; + tmp[1] = start_addr & 0xFF; + if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) + return -1; + + st.chip_cfg.dmp_loaded = 1; + st.chip_cfg.dmp_sample_rate = sample_rate; + return 0; +} + +/** + * @brief Enable/disable DMP support. + * @param[in] enable 1 to turn on the DMP. + * @return 0 if successful. + */ +int mpu_set_dmp_state(unsigned char enable) +{ + unsigned char tmp; + if (st.chip_cfg.dmp_on == enable) + return 0; + + if (enable) { + if (!st.chip_cfg.dmp_loaded) + return -1; + /* Disable data ready interrupt. */ + set_int_enable(0); + /* Disable bypass mode. */ + mpu_set_bypass(0); + /* Keep constant sample rate, FIFO rate controlled by DMP. */ + mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); + /* Remove FIFO elements. */ + tmp = 0; + i2c_write(st.hw->addr, 0x23, 1, &tmp); + st.chip_cfg.dmp_on = 1; + /* Enable DMP interrupt. */ + set_int_enable(1); + mpu_reset_fifo(); + } else { + /* Disable DMP interrupt. */ + set_int_enable(0); + /* Restore FIFO settings. */ + tmp = st.chip_cfg.fifo_enable; + i2c_write(st.hw->addr, 0x23, 1, &tmp); + st.chip_cfg.dmp_on = 0; + mpu_reset_fifo(); + } + return 0; +} + +/** + * @brief Get DMP state. + * @param[out] enabled 1 if enabled. + * @return 0 if successful. + */ +int mpu_get_dmp_state(unsigned char *enabled) +{ + enabled[0] = st.chip_cfg.dmp_on; + return 0; +} + +#ifdef AK89xx_SECONDARY +/* This initialization is similar to the one in ak8975.c. */ +static int setup_compass(void) +{ + unsigned char data[4], akm_addr; + + mpu_set_bypass(1); + + /* Find compass. Possible addresses range from 0x0C to 0x0F. */ + for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { + int result; + result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); + if (!result && (data[0] == AKM_WHOAMI)) + break; + } + + if (akm_addr > 0x0F) { + /* TODO: Handle this case in all compass-related functions. */ + log_e("Compass not found.\n"); + return -1; + } + + st.chip_cfg.compass_addr = akm_addr; + + data[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1); + + data[0] = AKM_FUSE_ROM_ACCESS; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1); + + /* Get sensitivity adjustment data from fuse ROM. */ + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) + return -1; + st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; + st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; + st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; + + data[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1); + + mpu_set_bypass(0); + + /* Set up master mode, master clock, and ES bit. */ + data[0] = 0x40; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + + /* Slave 0 reads from AKM data registers. */ + data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; + if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) + return -1; + + /* Compass reads start at this register. */ + data[0] = AKM_REG_ST1; + if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) + return -1; + + /* Enable slave 0, 8-byte reads. */ + data[0] = BIT_SLAVE_EN | 8; + if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) + return -1; + + /* Slave 1 changes AKM measurement mode. */ + data[0] = st.chip_cfg.compass_addr; + if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) + return -1; + + /* AKM measurement mode register. */ + data[0] = AKM_REG_CNTL; + if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) + return -1; + + /* Enable slave 1, 1-byte writes. */ + data[0] = BIT_SLAVE_EN | 1; + if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) + return -1; + + /* Set slave 1 data. */ + data[0] = AKM_SINGLE_MEASUREMENT; + if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) + return -1; + + /* Trigger slave 0 and slave 1 actions at each sample. */ + data[0] = 0x03; + if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) + return -1; + +#ifdef MPU9150 + /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ + data[0] = BIT_I2C_MST_VDDIO; + if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) + return -1; +#endif + + return 0; +} +#endif + +/** + * @brief Read raw compass data. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_compass_reg(short *data, unsigned long *timestamp) +{ +#ifdef AK89xx_SECONDARY + unsigned char tmp[9]; + + if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) + return -1; + +#ifdef AK89xx_BYPASS + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) + return -1; + tmp[8] = AKM_SINGLE_MEASUREMENT; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) + return -1; +#else + if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) + return -1; +#endif + +#if defined AK8975_SECONDARY + /* AK8975 doesn't have the overrun error bit. */ + if (!(tmp[0] & AKM_DATA_READY)) + return -2; + if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) + return -3; +#elif defined AK8963_SECONDARY + /* AK8963 doesn't have the data read error bit. */ + if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) + return -2; + if (tmp[7] & AKM_OVERFLOW) + return -3; +#endif + data[0] = (tmp[2] << 8) | tmp[1]; + data[1] = (tmp[4] << 8) | tmp[3]; + data[2] = (tmp[6] << 8) | tmp[5]; + + data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; + data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; + data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; + + if (timestamp) + get_ms(timestamp); + return 0; +#else + return -1; +#endif +} + +/** + * @brief Get the compass full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_compass_fsr(unsigned short *fsr) +{ +#ifdef AK89xx_SECONDARY + fsr[0] = st.hw->compass_fsr; + return 0; +#else + return -1; +#endif +} + +/** + * @brief Enters LP accel motion interrupt mode. + * The behaviour of this feature is very different between the MPU6050 and the + * MPU6500. Each chip's version of this feature is explained below. + * + * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg + * increments. + * + * \n Low-power accel mode supports the following frequencies: + * \n 1.25Hz, 5Hz, 20Hz, 40Hz + * + * \n MPU6500: + * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference + * sample. The hardware monitors the accel data and detects any large change + * over a short period of time. + * + * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg + * increments. + * + * \n MPU6500 Low-power accel mode supports the following frequencies: + * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz + * + * \n\n NOTES: + * \n The driver will round down @e thresh to the nearest supported value if + * an unsupported threshold is selected. + * \n To select a fractional wake-up frequency, round down the value passed to + * @e lpa_freq. + * \n The MPU6500 does not support a delay parameter. If this function is used + * for the MPU6500, the value passed to @e time will be ignored. + * \n To disable this mode, set @e lpa_freq to zero. The driver will restore + * the previous configuration. + * + * @param[in] thresh Motion threshold in mg. + * @param[in] time Duration in milliseconds that the accel data must + * exceed @e thresh before motion is reported. + * @param[in] lpa_freq Minimum sampling rate, or zero to disable. + * @return 0 if successful. + */ +int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, + unsigned short lpa_freq) +{ + +#if defined MPU6500 + unsigned char data[3]; +#endif + if (lpa_freq) { +#if defined MPU6500 + unsigned char thresh_hw; + + /* 1LSb = 4mg. */ + if (thresh > 1020) + thresh_hw = 255; + else if (thresh < 4) + thresh_hw = 1; + else + thresh_hw = thresh >> 2; +#endif + + if (!time) + /* Minimum duration must be 1ms. */ + time = 1; + +#if defined MPU6500 + if (lpa_freq > 640) + /* At this point, the chip has not been re-configured, so the + * function can safely exit. + */ + return -1; +#endif + + if (!st.chip_cfg.int_motion_only) { + /* Store current settings for later. */ + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + st.chip_cfg.cache.dmp_on = 1; + } else + st.chip_cfg.cache.dmp_on = 0; + mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); + mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); + mpu_get_lpf(&st.chip_cfg.cache.lpf); + mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); + st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); + } + +#if defined MPU6500 + /* Disable hardware interrupts. */ + set_int_enable(0); + + /* Enter full-power accel-only mode, no FIFO/DMP. */ + data[0] = 0; + data[1] = 0; + data[2] = BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data)) + goto lp_int_restore; + + /* Set motion threshold. */ + data[0] = thresh_hw; + if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data)) + goto lp_int_restore; + + /* Set wake frequency. */ + if (lpa_freq == 1) + data[0] = INV_LPA_1_25HZ; + else if (lpa_freq == 2) + data[0] = INV_LPA_2_5HZ; + else if (lpa_freq <= 5) + data[0] = INV_LPA_5HZ; + else if (lpa_freq <= 10) + data[0] = INV_LPA_10HZ; + else if (lpa_freq <= 20) + data[0] = INV_LPA_20HZ; + else if (lpa_freq <= 40) + data[0] = INV_LPA_40HZ; + else if (lpa_freq <= 80) + data[0] = INV_LPA_80HZ; + else if (lpa_freq <= 160) + data[0] = INV_LPA_160HZ; + else if (lpa_freq <= 320) + data[0] = INV_LPA_320HZ; + else + data[0] = INV_LPA_640HZ; + if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) + goto lp_int_restore; + + /* Enable motion interrupt (MPU6500 version). */ + data[0] = BITS_WOM_EN; + if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) + goto lp_int_restore; + + /* Enable cycle mode. */ + data[0] = BIT_LPA_CYCLE; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + goto lp_int_restore; + + /* Enable interrupt. */ + data[0] = BIT_MOT_INT_EN; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + goto lp_int_restore; + + st.chip_cfg.int_motion_only = 1; + return 0; +#endif + } else { + /* Don't "restore" the previous state if no state has been saved. */ + unsigned int ii; + char *cache_ptr = (char*)&st.chip_cfg.cache; + for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { + if (cache_ptr[ii] != 0) + goto lp_int_restore; + } + /* If we reach this point, motion interrupt mode hasn't been used yet. */ + return -1; + } +lp_int_restore: + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_sensors(st.chip_cfg.cache.sensors_on); + mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); + mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); + mpu_set_lpf(st.chip_cfg.cache.lpf); + mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); + mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); + + if (st.chip_cfg.cache.dmp_on) + mpu_set_dmp_state(1); + +#ifdef MPU6500 + /* Disable motion interrupt (MPU6500 version). */ + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) + goto lp_int_restore; +#endif + + st.chip_cfg.int_motion_only = 0; + return 0; +} + +/** + * @} + */ + +