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
inv_mpu.c
- Committer:
- mbedoguz
- Date:
- 2017-08-14
- Revision:
- 6:7469a85601f1
- Parent:
- 1:a6c3f8680fe0
File content as of revision 6:7469a85601f1:
/* $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 #warning Your system is not officially supported. So, I will just include \ mdcompat.h, however you must provide it. #include "mdcompat.h" //#include "log_mbed.h" #define log_i(...) do {} while (0) #define log_e(...) do {} while (0) #define i2c_read mbed_i2c_read #define i2c_write mbed_i2c_write #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 /* 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. */ 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) //nsolreg_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; } /** * @} */