Yihui Xiong / eMPL_MPU

library for MPU6050 and MPU9250, supports both I2C and SPI

Dependents:   Seeed_nRF51822_MPU9250

inv_mpu.c@0:972f3778c19c, 2015-12-10 (annotated)

Committer:
yihui
Date:
Thu Dec 10 07:39:48 2015 +0000
Revision:
0:972f3778c19c
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:972f3778c19c 1 /*
yihui 0:972f3778c19c 2 $License:
yihui 0:972f3778c19c 3 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
yihui 0:972f3778c19c 4 See included License.txt for License information.
yihui 0:972f3778c19c 5 $
yihui 0:972f3778c19c 6 */
yihui 0:972f3778c19c 7 /**
yihui 0:972f3778c19c 8 * @addtogroup DRIVERS Sensor Driver Layer
yihui 0:972f3778c19c 9 * @brief Hardware drivers to communicate with sensors via I2C.
yihui 0:972f3778c19c 10 *
yihui 0:972f3778c19c 11 * @{
yihui 0:972f3778c19c 12 * @file inv_mpu.c
yihui 0:972f3778c19c 13 * @brief An I2C-based driver for Invensense gyroscopes.
yihui 0:972f3778c19c 14 * @details This driver currently works for the following devices:
yihui 0:972f3778c19c 15 * MPU6050
yihui 0:972f3778c19c 16 * MPU6500
yihui 0:972f3778c19c 17 * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)
yihui 0:972f3778c19c 18 * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)
yihui 0:972f3778c19c 19 */
yihui 0:972f3778c19c 20 #include <stdio.h>
yihui 0:972f3778c19c 21 #include <stdint.h>
yihui 0:972f3778c19c 22 #include <stdlib.h>
yihui 0:972f3778c19c 23 #include <string.h>
yihui 0:972f3778c19c 24 #include <math.h>
yihui 0:972f3778c19c 25 #include "inv_mpu.h"
yihui 0:972f3778c19c 26
yihui 0:972f3778c19c 27 /* The following functions must be defined for this platform:
yihui 0:972f3778c19c 28 * mpu_hal_write(unsigned char slave_addr, unsigned char reg_addr,
yihui 0:972f3778c19c 29 * unsigned char length, unsigned char const *data)
yihui 0:972f3778c19c 30 * mpu_hal_read(unsigned char slave_addr, unsigned char reg_addr,
yihui 0:972f3778c19c 31 * unsigned char length, unsigned char *data)
yihui 0:972f3778c19c 32 * delay_ms(unsigned long num_ms)
yihui 0:972f3778c19c 33 * get_ms(unsigned long *count)
yihui 0:972f3778c19c 34 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
yihui 0:972f3778c19c 35 * labs(long x)
yihui 0:972f3778c19c 36 * fabsf(float x)
yihui 0:972f3778c19c 37 * min(int a, int b)
yihui 0:972f3778c19c 38 */
yihui 0:972f3778c19c 39 #if defined __MBED__ // for mbed platform
yihui 0:972f3778c19c 40 #include "mpu_mbed_config.h"
yihui 0:972f3778c19c 41 #elif defined MOTION_DRIVER_TARGET_MSP430
yihui 0:972f3778c19c 42 #include "msp430.h"
yihui 0:972f3778c19c 43 #include "msp430_i2c.h"
yihui 0:972f3778c19c 44 #include "msp430_clock.h"
yihui 0:972f3778c19c 45 #include "msp430_interrupt.h"
yihui 0:972f3778c19c 46 #define mpu_hal_write msp430_mpu_hal_write
yihui 0:972f3778c19c 47 #define mpu_hal_read msp430_mpu_hal_read
yihui 0:972f3778c19c 48 #define delay_ms msp430_delay_ms
yihui 0:972f3778c19c 49 #define get_ms msp430_get_clock_ms
yihui 0:972f3778c19c 50 static inline int reg_int_cb(struct int_param_s *int_param)
yihui 0:972f3778c19c 51 {
yihui 0:972f3778c19c 52 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
yihui 0:972f3778c19c 53 int_param->active_low);
yihui 0:972f3778c19c 54 }
yihui 0:972f3778c19c 55 #define log_i(...) do {} while (0)
yihui 0:972f3778c19c 56 #define log_e(...) do {} while (0)
yihui 0:972f3778c19c 57 /* labs is already defined by TI's toolchain. */
yihui 0:972f3778c19c 58 /* fabs is for doubles. fabsf is for floats. */
yihui 0:972f3778c19c 59 #define fabs fabsf
yihui 0:972f3778c19c 60 #define min(a,b) ((a<b)?a:b)
yihui 0:972f3778c19c 61 #elif defined EMPL_TARGET_MSP430
yihui 0:972f3778c19c 62 #include "msp430.h"
yihui 0:972f3778c19c 63 #include "msp430_i2c.h"
yihui 0:972f3778c19c 64 #include "msp430_clock.h"
yihui 0:972f3778c19c 65 #include "msp430_interrupt.h"
yihui 0:972f3778c19c 66 #include "log.h"
yihui 0:972f3778c19c 67 #define mpu_hal_write msp430_mpu_hal_write
yihui 0:972f3778c19c 68 #define mpu_hal_read msp430_mpu_hal_read
yihui 0:972f3778c19c 69 #define delay_ms msp430_delay_ms
yihui 0:972f3778c19c 70 #define get_ms msp430_get_clock_ms
yihui 0:972f3778c19c 71 static inline int reg_int_cb(struct int_param_s *int_param)
yihui 0:972f3778c19c 72 {
yihui 0:972f3778c19c 73 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
yihui 0:972f3778c19c 74 int_param->active_low);
yihui 0:972f3778c19c 75 }
yihui 0:972f3778c19c 76 #define log_i MPL_LOGI
yihui 0:972f3778c19c 77 #define log_e MPL_LOGE
yihui 0:972f3778c19c 78 /* labs is already defined by TI's toolchain. */
yihui 0:972f3778c19c 79 /* fabs is for doubles. fabsf is for floats. */
yihui 0:972f3778c19c 80 #define fabs fabsf
yihui 0:972f3778c19c 81 #define min(a,b) ((a<b)?a:b)
yihui 0:972f3778c19c 82 #elif defined EMPL_TARGET_UC3L0
yihui 0:972f3778c19c 83 /* Instead of using the standard TWI driver from the ASF library, we're using
yihui 0:972f3778c19c 84 * a TWI driver that follows the slave address + register address convention.
yihui 0:972f3778c19c 85 */
yihui 0:972f3778c19c 86 #include "twi.h"
yihui 0:972f3778c19c 87 #include "delay.h"
yihui 0:972f3778c19c 88 #include "sysclk.h"
yihui 0:972f3778c19c 89 #include "log.h"
yihui 0:972f3778c19c 90 #include "sensors_xplained.h"
yihui 0:972f3778c19c 91 #include "uc3l0_clock.h"
yihui 0:972f3778c19c 92 #define mpu_hal_write(a, b, c, d) twi_write(a, b, d, c)
yihui 0:972f3778c19c 93 #define mpu_hal_read(a, b, c, d) twi_read(a, b, d, c)
yihui 0:972f3778c19c 94 /* delay_ms is a function already defined in ASF. */
yihui 0:972f3778c19c 95 #define get_ms uc3l0_get_clock_ms
yihui 0:972f3778c19c 96 static inline int reg_int_cb(struct int_param_s *int_param)
yihui 0:972f3778c19c 97 {
yihui 0:972f3778c19c 98 sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
yihui 0:972f3778c19c 99 return 0;
yihui 0:972f3778c19c 100 }
yihui 0:972f3778c19c 101 #define log_i MPL_LOGI
yihui 0:972f3778c19c 102 #define log_e MPL_LOGE
yihui 0:972f3778c19c 103 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */
yihui 0:972f3778c19c 104 #define labs abs
yihui 0:972f3778c19c 105 #define fabs(x) (((x)>0)?(x):-(x))
yihui 0:972f3778c19c 106
yihui 0:972f3778c19c 107 #else
yihui 0:972f3778c19c 108 #error Gyro driver is missing the system layer implementations.
yihui 0:972f3778c19c 109 #endif
yihui 0:972f3778c19c 110
yihui 0:972f3778c19c 111 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
yihui 0:972f3778c19c 112 #error Which gyro are you using? Define MPUxxxx in your compiler options.
yihui 0:972f3778c19c 113 #endif
yihui 0:972f3778c19c 114
yihui 0:972f3778c19c 115 /* Time for some messy macro work. =]
yihui 0:972f3778c19c 116 * #define MPU9150
yihui 0:972f3778c19c 117 * is equivalent to..
yihui 0:972f3778c19c 118 * #define MPU6050
yihui 0:972f3778c19c 119 * #define AK8975_SECONDARY
yihui 0:972f3778c19c 120 *
yihui 0:972f3778c19c 121 * #define MPU9250
yihui 0:972f3778c19c 122 * is equivalent to..
yihui 0:972f3778c19c 123 * #define MPU6500
yihui 0:972f3778c19c 124 * #define AK8963_SECONDARY
yihui 0:972f3778c19c 125 */
yihui 0:972f3778c19c 126 #if defined MPU9150
yihui 0:972f3778c19c 127 #ifndef MPU6050
yihui 0:972f3778c19c 128 #define MPU6050
yihui 0:972f3778c19c 129 #endif /* #ifndef MPU6050 */
yihui 0:972f3778c19c 130 #if defined AK8963_SECONDARY
yihui 0:972f3778c19c 131 #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
yihui 0:972f3778c19c 132 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
yihui 0:972f3778c19c 133 #define AK8975_SECONDARY
yihui 0:972f3778c19c 134 #endif /* #if defined AK8963_SECONDARY */
yihui 0:972f3778c19c 135 #elif defined MPU9250 /* #if defined MPU9150 */
yihui 0:972f3778c19c 136 #ifndef MPU6500
yihui 0:972f3778c19c 137 #define MPU6500
yihui 0:972f3778c19c 138 #endif /* #ifndef MPU6500 */
yihui 0:972f3778c19c 139 #if defined AK8975_SECONDARY
yihui 0:972f3778c19c 140 #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
yihui 0:972f3778c19c 141 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
yihui 0:972f3778c19c 142 #define AK8963_SECONDARY
yihui 0:972f3778c19c 143 #endif /* #if defined AK8975_SECONDARY */
yihui 0:972f3778c19c 144 #endif /* #if defined MPU9150 */
yihui 0:972f3778c19c 145
yihui 0:972f3778c19c 146 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY
yihui 0:972f3778c19c 147 #define AK89xx_SECONDARY
yihui 0:972f3778c19c 148 #else
yihui 0:972f3778c19c 149 /* #warning "No compass = less profit for Invensense. Lame." */
yihui 0:972f3778c19c 150 #endif
yihui 0:972f3778c19c 151
yihui 0:972f3778c19c 152 static int set_int_enable(unsigned char enable);
yihui 0:972f3778c19c 153
yihui 0:972f3778c19c 154 /* Hardware registers needed by driver. */
yihui 0:972f3778c19c 155 struct gyro_reg_s {
yihui 0:972f3778c19c 156 unsigned char who_am_i;
yihui 0:972f3778c19c 157 unsigned char rate_div;
yihui 0:972f3778c19c 158 unsigned char lpf;
yihui 0:972f3778c19c 159 unsigned char prod_id;
yihui 0:972f3778c19c 160 unsigned char user_ctrl;
yihui 0:972f3778c19c 161 unsigned char fifo_en;
yihui 0:972f3778c19c 162 unsigned char gyro_cfg;
yihui 0:972f3778c19c 163 unsigned char accel_cfg;
yihui 0:972f3778c19c 164 unsigned char accel_cfg2;
yihui 0:972f3778c19c 165 unsigned char lp_accel_odr;
yihui 0:972f3778c19c 166 unsigned char motion_thr;
yihui 0:972f3778c19c 167 unsigned char motion_dur;
yihui 0:972f3778c19c 168 unsigned char fifo_count_h;
yihui 0:972f3778c19c 169 unsigned char fifo_r_w;
yihui 0:972f3778c19c 170 unsigned char raw_gyro;
yihui 0:972f3778c19c 171 unsigned char raw_accel;
yihui 0:972f3778c19c 172 unsigned char temp;
yihui 0:972f3778c19c 173 unsigned char int_enable;
yihui 0:972f3778c19c 174 unsigned char dmp_int_status;
yihui 0:972f3778c19c 175 unsigned char int_status;
yihui 0:972f3778c19c 176 unsigned char accel_intel;
yihui 0:972f3778c19c 177 unsigned char pwr_mgmt_1;
yihui 0:972f3778c19c 178 unsigned char pwr_mgmt_2;
yihui 0:972f3778c19c 179 unsigned char int_pin_cfg;
yihui 0:972f3778c19c 180 unsigned char mem_r_w;
yihui 0:972f3778c19c 181 unsigned char accel_offs;
yihui 0:972f3778c19c 182 unsigned char i2c_mst;
yihui 0:972f3778c19c 183 unsigned char bank_sel;
yihui 0:972f3778c19c 184 unsigned char mem_start_addr;
yihui 0:972f3778c19c 185 unsigned char prgm_start_h;
yihui 0:972f3778c19c 186 #if defined AK89xx_SECONDARY
yihui 0:972f3778c19c 187 unsigned char s0_addr;
yihui 0:972f3778c19c 188 unsigned char s0_reg;
yihui 0:972f3778c19c 189 unsigned char s0_ctrl;
yihui 0:972f3778c19c 190 unsigned char s1_addr;
yihui 0:972f3778c19c 191 unsigned char s1_reg;
yihui 0:972f3778c19c 192 unsigned char s1_ctrl;
yihui 0:972f3778c19c 193 unsigned char s4_ctrl;
yihui 0:972f3778c19c 194 unsigned char s0_do;
yihui 0:972f3778c19c 195 unsigned char s1_do;
yihui 0:972f3778c19c 196 unsigned char i2c_delay_ctrl;
yihui 0:972f3778c19c 197 unsigned char raw_compass;
yihui 0:972f3778c19c 198 /* The I2C_MST_VDDIO bit is in this register. */
yihui 0:972f3778c19c 199 unsigned char yg_offs_tc;
yihui 0:972f3778c19c 200 #endif
yihui 0:972f3778c19c 201 };
yihui 0:972f3778c19c 202
yihui 0:972f3778c19c 203 /* Information specific to a particular device. */
yihui 0:972f3778c19c 204 struct hw_s {
yihui 0:972f3778c19c 205 unsigned char addr;
yihui 0:972f3778c19c 206 unsigned short max_fifo;
yihui 0:972f3778c19c 207 unsigned char num_reg;
yihui 0:972f3778c19c 208 unsigned short temp_sens;
yihui 0:972f3778c19c 209 short temp_offset;
yihui 0:972f3778c19c 210 unsigned short bank_size;
yihui 0:972f3778c19c 211 #if defined AK89xx_SECONDARY
yihui 0:972f3778c19c 212 unsigned short compass_fsr;
yihui 0:972f3778c19c 213 #endif
yihui 0:972f3778c19c 214 };
yihui 0:972f3778c19c 215
yihui 0:972f3778c19c 216 /* When entering motion interrupt mode, the driver keeps track of the
yihui 0:972f3778c19c 217 * previous state so that it can be restored at a later time.
yihui 0:972f3778c19c 218 * TODO: This is tacky. Fix it.
yihui 0:972f3778c19c 219 */
yihui 0:972f3778c19c 220 struct motion_int_cache_s {
yihui 0:972f3778c19c 221 unsigned short gyro_fsr;
yihui 0:972f3778c19c 222 unsigned char accel_fsr;
yihui 0:972f3778c19c 223 unsigned short lpf;
yihui 0:972f3778c19c 224 unsigned short sample_rate;
yihui 0:972f3778c19c 225 unsigned char sensors_on;
yihui 0:972f3778c19c 226 unsigned char fifo_sensors;
yihui 0:972f3778c19c 227 unsigned char dmp_on;
yihui 0:972f3778c19c 228 };
yihui 0:972f3778c19c 229
yihui 0:972f3778c19c 230 /* Cached chip configuration data.
yihui 0:972f3778c19c 231 * TODO: A lot of these can be handled with a bitmask.
yihui 0:972f3778c19c 232 */
yihui 0:972f3778c19c 233 struct chip_cfg_s {
yihui 0:972f3778c19c 234 /* Matches gyro_cfg >> 3 & 0x03 */
yihui 0:972f3778c19c 235 unsigned char gyro_fsr;
yihui 0:972f3778c19c 236 /* Matches accel_cfg >> 3 & 0x03 */
yihui 0:972f3778c19c 237 unsigned char accel_fsr;
yihui 0:972f3778c19c 238 /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
yihui 0:972f3778c19c 239 unsigned char sensors;
yihui 0:972f3778c19c 240 /* Matches config register. */
yihui 0:972f3778c19c 241 unsigned char lpf;
yihui 0:972f3778c19c 242 unsigned char clk_src;
yihui 0:972f3778c19c 243 /* Sample rate, NOT rate divider. */
yihui 0:972f3778c19c 244 unsigned short sample_rate;
yihui 0:972f3778c19c 245 /* Matches fifo_en register. */
yihui 0:972f3778c19c 246 unsigned char fifo_enable;
yihui 0:972f3778c19c 247 /* Matches int enable register. */
yihui 0:972f3778c19c 248 unsigned char int_enable;
yihui 0:972f3778c19c 249 /* 1 if devices on auxiliary I2C bus appear on the primary. */
yihui 0:972f3778c19c 250 unsigned char bypass_mode;
yihui 0:972f3778c19c 251 /* 1 if half-sensitivity.
yihui 0:972f3778c19c 252 * NOTE: This doesn't belong here, but everything else in hw_s is const,
yihui 0:972f3778c19c 253 * and this allows us to save some precious RAM.
yihui 0:972f3778c19c 254 */
yihui 0:972f3778c19c 255 unsigned char accel_half;
yihui 0:972f3778c19c 256 /* 1 if device in low-power accel-only mode. */
yihui 0:972f3778c19c 257 unsigned char lp_accel_mode;
yihui 0:972f3778c19c 258 /* 1 if interrupts are only triggered on motion events. */
yihui 0:972f3778c19c 259 unsigned char int_motion_only;
yihui 0:972f3778c19c 260 struct motion_int_cache_s cache;
yihui 0:972f3778c19c 261 /* 1 for active low interrupts. */
yihui 0:972f3778c19c 262 unsigned char active_low_int;
yihui 0:972f3778c19c 263 /* 1 for latched interrupts. */
yihui 0:972f3778c19c 264 unsigned char latched_int;
yihui 0:972f3778c19c 265 /* 1 if DMP is enabled. */
yihui 0:972f3778c19c 266 unsigned char dmp_on;
yihui 0:972f3778c19c 267 /* Ensures that DMP will only be loaded once. */
yihui 0:972f3778c19c 268 unsigned char dmp_loaded;
yihui 0:972f3778c19c 269 /* Sampling rate used when DMP is enabled. */
yihui 0:972f3778c19c 270 unsigned short dmp_sample_rate;
yihui 0:972f3778c19c 271 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 272 /* Compass sample rate. */
yihui 0:972f3778c19c 273 unsigned short compass_sample_rate;
yihui 0:972f3778c19c 274 unsigned char compass_addr;
yihui 0:972f3778c19c 275 short mag_sens_adj[3];
yihui 0:972f3778c19c 276 #endif
yihui 0:972f3778c19c 277 };
yihui 0:972f3778c19c 278
yihui 0:972f3778c19c 279 /* Information for self-test. */
yihui 0:972f3778c19c 280 struct test_s {
yihui 0:972f3778c19c 281 unsigned long gyro_sens;
yihui 0:972f3778c19c 282 unsigned long accel_sens;
yihui 0:972f3778c19c 283 unsigned char reg_rate_div;
yihui 0:972f3778c19c 284 unsigned char reg_lpf;
yihui 0:972f3778c19c 285 unsigned char reg_gyro_fsr;
yihui 0:972f3778c19c 286 unsigned char reg_accel_fsr;
yihui 0:972f3778c19c 287 unsigned short wait_ms;
yihui 0:972f3778c19c 288 unsigned char packet_thresh;
yihui 0:972f3778c19c 289 float min_dps;
yihui 0:972f3778c19c 290 float max_dps;
yihui 0:972f3778c19c 291 float max_gyro_var;
yihui 0:972f3778c19c 292 float min_g;
yihui 0:972f3778c19c 293 float max_g;
yihui 0:972f3778c19c 294 float max_accel_var;
yihui 0:972f3778c19c 295 #ifdef MPU6500
yihui 0:972f3778c19c 296 float max_g_offset;
yihui 0:972f3778c19c 297 unsigned short sample_wait_ms;
yihui 0:972f3778c19c 298 #endif
yihui 0:972f3778c19c 299 };
yihui 0:972f3778c19c 300
yihui 0:972f3778c19c 301 /* Gyro driver state variables. */
yihui 0:972f3778c19c 302 struct gyro_state_s {
yihui 0:972f3778c19c 303 const struct gyro_reg_s *reg;
yihui 0:972f3778c19c 304 const struct hw_s *hw;
yihui 0:972f3778c19c 305 struct chip_cfg_s chip_cfg;
yihui 0:972f3778c19c 306 const struct test_s *test;
yihui 0:972f3778c19c 307 };
yihui 0:972f3778c19c 308
yihui 0:972f3778c19c 309 /* Filter configurations. */
yihui 0:972f3778c19c 310 enum lpf_e {
yihui 0:972f3778c19c 311 INV_FILTER_256HZ_NOLPF2 = 0,
yihui 0:972f3778c19c 312 INV_FILTER_188HZ,
yihui 0:972f3778c19c 313 INV_FILTER_98HZ,
yihui 0:972f3778c19c 314 INV_FILTER_42HZ,
yihui 0:972f3778c19c 315 INV_FILTER_20HZ,
yihui 0:972f3778c19c 316 INV_FILTER_10HZ,
yihui 0:972f3778c19c 317 INV_FILTER_5HZ,
yihui 0:972f3778c19c 318 INV_FILTER_2100HZ_NOLPF,
yihui 0:972f3778c19c 319 NUM_FILTER
yihui 0:972f3778c19c 320 };
yihui 0:972f3778c19c 321
yihui 0:972f3778c19c 322 /* Full scale ranges. */
yihui 0:972f3778c19c 323 enum gyro_fsr_e {
yihui 0:972f3778c19c 324 INV_FSR_250DPS = 0,
yihui 0:972f3778c19c 325 INV_FSR_500DPS,
yihui 0:972f3778c19c 326 INV_FSR_1000DPS,
yihui 0:972f3778c19c 327 INV_FSR_2000DPS,
yihui 0:972f3778c19c 328 NUM_GYRO_FSR
yihui 0:972f3778c19c 329 };
yihui 0:972f3778c19c 330
yihui 0:972f3778c19c 331 /* Full scale ranges. */
yihui 0:972f3778c19c 332 enum accel_fsr_e {
yihui 0:972f3778c19c 333 INV_FSR_2G = 0,
yihui 0:972f3778c19c 334 INV_FSR_4G,
yihui 0:972f3778c19c 335 INV_FSR_8G,
yihui 0:972f3778c19c 336 INV_FSR_16G,
yihui 0:972f3778c19c 337 NUM_ACCEL_FSR
yihui 0:972f3778c19c 338 };
yihui 0:972f3778c19c 339
yihui 0:972f3778c19c 340 /* Clock sources. */
yihui 0:972f3778c19c 341 enum clock_sel_e {
yihui 0:972f3778c19c 342 INV_CLK_INTERNAL = 0,
yihui 0:972f3778c19c 343 INV_CLK_PLL,
yihui 0:972f3778c19c 344 NUM_CLK
yihui 0:972f3778c19c 345 };
yihui 0:972f3778c19c 346
yihui 0:972f3778c19c 347 /* Low-power accel wakeup rates. */
yihui 0:972f3778c19c 348 enum lp_accel_rate_e {
yihui 0:972f3778c19c 349 #if defined MPU6050
yihui 0:972f3778c19c 350 INV_LPA_1_25HZ,
yihui 0:972f3778c19c 351 INV_LPA_5HZ,
yihui 0:972f3778c19c 352 INV_LPA_20HZ,
yihui 0:972f3778c19c 353 INV_LPA_40HZ
yihui 0:972f3778c19c 354 #elif defined MPU6500
yihui 0:972f3778c19c 355 INV_LPA_0_3125HZ,
yihui 0:972f3778c19c 356 INV_LPA_0_625HZ,
yihui 0:972f3778c19c 357 INV_LPA_1_25HZ,
yihui 0:972f3778c19c 358 INV_LPA_2_5HZ,
yihui 0:972f3778c19c 359 INV_LPA_5HZ,
yihui 0:972f3778c19c 360 INV_LPA_10HZ,
yihui 0:972f3778c19c 361 INV_LPA_20HZ,
yihui 0:972f3778c19c 362 INV_LPA_40HZ,
yihui 0:972f3778c19c 363 INV_LPA_80HZ,
yihui 0:972f3778c19c 364 INV_LPA_160HZ,
yihui 0:972f3778c19c 365 INV_LPA_320HZ,
yihui 0:972f3778c19c 366 INV_LPA_640HZ
yihui 0:972f3778c19c 367 #endif
yihui 0:972f3778c19c 368 };
yihui 0:972f3778c19c 369
yihui 0:972f3778c19c 370 #define BIT_I2C_MST_VDDIO (0x80)
yihui 0:972f3778c19c 371 #define BIT_FIFO_EN (0x40)
yihui 0:972f3778c19c 372 #define BIT_DMP_EN (0x80)
yihui 0:972f3778c19c 373 #define BIT_FIFO_RST (0x04)
yihui 0:972f3778c19c 374 #define BIT_DMP_RST (0x08)
yihui 0:972f3778c19c 375 #define BIT_FIFO_OVERFLOW (0x10)
yihui 0:972f3778c19c 376 #define BIT_DATA_RDY_EN (0x01)
yihui 0:972f3778c19c 377 #define BIT_DMP_INT_EN (0x02)
yihui 0:972f3778c19c 378 #define BIT_MOT_INT_EN (0x40)
yihui 0:972f3778c19c 379 #define BITS_FSR (0x18)
yihui 0:972f3778c19c 380 #define BITS_LPF (0x07)
yihui 0:972f3778c19c 381 #define BITS_HPF (0x07)
yihui 0:972f3778c19c 382 #define BITS_CLK (0x07)
yihui 0:972f3778c19c 383 #define BIT_FIFO_SIZE_1024 (0x40)
yihui 0:972f3778c19c 384 #define BIT_FIFO_SIZE_2048 (0x80)
yihui 0:972f3778c19c 385 #define BIT_FIFO_SIZE_4096 (0xC0)
yihui 0:972f3778c19c 386 #define BIT_RESET (0x80)
yihui 0:972f3778c19c 387 #define BIT_SLEEP (0x40)
yihui 0:972f3778c19c 388 #define BIT_S0_DELAY_EN (0x01)
yihui 0:972f3778c19c 389 #define BIT_S2_DELAY_EN (0x04)
yihui 0:972f3778c19c 390 #define BITS_SLAVE_LENGTH (0x0F)
yihui 0:972f3778c19c 391 #define BIT_SLAVE_BYTE_SW (0x40)
yihui 0:972f3778c19c 392 #define BIT_SLAVE_GROUP (0x10)
yihui 0:972f3778c19c 393 #define BIT_SLAVE_EN (0x80)
yihui 0:972f3778c19c 394 #define BIT_I2C_READ (0x80)
yihui 0:972f3778c19c 395 #define BITS_I2C_MASTER_DLY (0x1F)
yihui 0:972f3778c19c 396 #define BIT_AUX_IF_EN (0x20)
yihui 0:972f3778c19c 397 #define BIT_ACTL (0x80)
yihui 0:972f3778c19c 398 #define BIT_LATCH_EN (0x20)
yihui 0:972f3778c19c 399 #define BIT_ANY_RD_CLR (0x10)
yihui 0:972f3778c19c 400 #define BIT_BYPASS_EN (0x02)
yihui 0:972f3778c19c 401 #define BITS_WOM_EN (0xC0)
yihui 0:972f3778c19c 402 #define BIT_LPA_CYCLE (0x20)
yihui 0:972f3778c19c 403 #define BIT_STBY_XA (0x20)
yihui 0:972f3778c19c 404 #define BIT_STBY_YA (0x10)
yihui 0:972f3778c19c 405 #define BIT_STBY_ZA (0x08)
yihui 0:972f3778c19c 406 #define BIT_STBY_XG (0x04)
yihui 0:972f3778c19c 407 #define BIT_STBY_YG (0x02)
yihui 0:972f3778c19c 408 #define BIT_STBY_ZG (0x01)
yihui 0:972f3778c19c 409 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
yihui 0:972f3778c19c 410 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
yihui 0:972f3778c19c 411
yihui 0:972f3778c19c 412 #if defined AK8975_SECONDARY
yihui 0:972f3778c19c 413 #define SUPPORTS_AK89xx_HIGH_SENS (0x00)
yihui 0:972f3778c19c 414 #define AK89xx_FSR (9830)
yihui 0:972f3778c19c 415 #elif defined AK8963_SECONDARY
yihui 0:972f3778c19c 416 #define SUPPORTS_AK89xx_HIGH_SENS (0x10)
yihui 0:972f3778c19c 417 #define AK89xx_FSR (4915)
yihui 0:972f3778c19c 418 #endif
yihui 0:972f3778c19c 419
yihui 0:972f3778c19c 420 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 421 #define AKM_REG_WHOAMI (0x00)
yihui 0:972f3778c19c 422
yihui 0:972f3778c19c 423 #define AKM_REG_ST1 (0x02)
yihui 0:972f3778c19c 424 #define AKM_REG_HXL (0x03)
yihui 0:972f3778c19c 425 #define AKM_REG_ST2 (0x09)
yihui 0:972f3778c19c 426
yihui 0:972f3778c19c 427 #define AKM_REG_CNTL (0x0A)
yihui 0:972f3778c19c 428 #define AKM_REG_ASTC (0x0C)
yihui 0:972f3778c19c 429 #define AKM_REG_ASAX (0x10)
yihui 0:972f3778c19c 430 #define AKM_REG_ASAY (0x11)
yihui 0:972f3778c19c 431 #define AKM_REG_ASAZ (0x12)
yihui 0:972f3778c19c 432
yihui 0:972f3778c19c 433 #define AKM_DATA_READY (0x01)
yihui 0:972f3778c19c 434 #define AKM_DATA_OVERRUN (0x02)
yihui 0:972f3778c19c 435 #define AKM_OVERFLOW (0x80)
yihui 0:972f3778c19c 436 #define AKM_DATA_ERROR (0x40)
yihui 0:972f3778c19c 437
yihui 0:972f3778c19c 438 #define AKM_BIT_SELF_TEST (0x40)
yihui 0:972f3778c19c 439
yihui 0:972f3778c19c 440 #define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
yihui 0:972f3778c19c 441 #define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
yihui 0:972f3778c19c 442 #define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
yihui 0:972f3778c19c 443 #define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
yihui 0:972f3778c19c 444
yihui 0:972f3778c19c 445 #define AKM_WHOAMI (0x48)
yihui 0:972f3778c19c 446 #endif
yihui 0:972f3778c19c 447
yihui 0:972f3778c19c 448 #if defined MPU6050
yihui 0:972f3778c19c 449 const struct gyro_reg_s reg = {
yihui 0:972f3778c19c 450 .who_am_i = 0x75,
yihui 0:972f3778c19c 451 .rate_div = 0x19,
yihui 0:972f3778c19c 452 .lpf = 0x1A,
yihui 0:972f3778c19c 453 .prod_id = 0x0C,
yihui 0:972f3778c19c 454 .user_ctrl = 0x6A,
yihui 0:972f3778c19c 455 .fifo_en = 0x23,
yihui 0:972f3778c19c 456 .gyro_cfg = 0x1B,
yihui 0:972f3778c19c 457 .accel_cfg = 0x1C,
yihui 0:972f3778c19c 458 .motion_thr = 0x1F,
yihui 0:972f3778c19c 459 .motion_dur = 0x20,
yihui 0:972f3778c19c 460 .fifo_count_h = 0x72,
yihui 0:972f3778c19c 461 .fifo_r_w = 0x74,
yihui 0:972f3778c19c 462 .raw_gyro = 0x43,
yihui 0:972f3778c19c 463 .raw_accel = 0x3B,
yihui 0:972f3778c19c 464 .temp = 0x41,
yihui 0:972f3778c19c 465 .int_enable = 0x38,
yihui 0:972f3778c19c 466 .dmp_int_status = 0x39,
yihui 0:972f3778c19c 467 .int_status = 0x3A,
yihui 0:972f3778c19c 468 .pwr_mgmt_1 = 0x6B,
yihui 0:972f3778c19c 469 .pwr_mgmt_2 = 0x6C,
yihui 0:972f3778c19c 470 .int_pin_cfg = 0x37,
yihui 0:972f3778c19c 471 .mem_r_w = 0x6F,
yihui 0:972f3778c19c 472 .accel_offs = 0x06,
yihui 0:972f3778c19c 473 .i2c_mst = 0x24,
yihui 0:972f3778c19c 474 .bank_sel = 0x6D,
yihui 0:972f3778c19c 475 .mem_start_addr = 0x6E,
yihui 0:972f3778c19c 476 .prgm_start_h = 0x70
yihui 0:972f3778c19c 477 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 478 ,.raw_compass = 0x49,
yihui 0:972f3778c19c 479 .yg_offs_tc = 0x01,
yihui 0:972f3778c19c 480 .s0_addr = 0x25,
yihui 0:972f3778c19c 481 .s0_reg = 0x26,
yihui 0:972f3778c19c 482 .s0_ctrl = 0x27,
yihui 0:972f3778c19c 483 .s1_addr = 0x28,
yihui 0:972f3778c19c 484 .s1_reg = 0x29,
yihui 0:972f3778c19c 485 .s1_ctrl = 0x2A,
yihui 0:972f3778c19c 486 .s4_ctrl = 0x34,
yihui 0:972f3778c19c 487 .s0_do = 0x63,
yihui 0:972f3778c19c 488 .s1_do = 0x64,
yihui 0:972f3778c19c 489 .i2c_delay_ctrl = 0x67
yihui 0:972f3778c19c 490 #endif
yihui 0:972f3778c19c 491 };
yihui 0:972f3778c19c 492 const struct hw_s hw = {
yihui 0:972f3778c19c 493 .addr = 0x69,
yihui 0:972f3778c19c 494 .max_fifo = 1024,
yihui 0:972f3778c19c 495 .num_reg = 118,
yihui 0:972f3778c19c 496 .temp_sens = 340,
yihui 0:972f3778c19c 497 .temp_offset = -521,
yihui 0:972f3778c19c 498 .bank_size = 256
yihui 0:972f3778c19c 499 #if defined AK89xx_SECONDARY
yihui 0:972f3778c19c 500 ,.compass_fsr = AK89xx_FSR
yihui 0:972f3778c19c 501 #endif
yihui 0:972f3778c19c 502 };
yihui 0:972f3778c19c 503
yihui 0:972f3778c19c 504 const struct test_s test = {
yihui 0:972f3778c19c 505 .gyro_sens = 32768/250,
yihui 0:972f3778c19c 506 .accel_sens = 32768/16,
yihui 0:972f3778c19c 507 .reg_rate_div = 0, /* 1kHz. */
yihui 0:972f3778c19c 508 .reg_lpf = 1, /* 188Hz. */
yihui 0:972f3778c19c 509 .reg_gyro_fsr = 0, /* 250dps. */
yihui 0:972f3778c19c 510 .reg_accel_fsr = 0x18, /* 16g. */
yihui 0:972f3778c19c 511 .wait_ms = 50,
yihui 0:972f3778c19c 512 .packet_thresh = 5, /* 5% */
yihui 0:972f3778c19c 513 .min_dps = 10.f,
yihui 0:972f3778c19c 514 .max_dps = 105.f,
yihui 0:972f3778c19c 515 .max_gyro_var = 0.14f,
yihui 0:972f3778c19c 516 .min_g = 0.3f,
yihui 0:972f3778c19c 517 .max_g = 0.95f,
yihui 0:972f3778c19c 518 .max_accel_var = 0.14f
yihui 0:972f3778c19c 519 };
yihui 0:972f3778c19c 520
yihui 0:972f3778c19c 521 static struct gyro_state_s st = {
yihui 0:972f3778c19c 522 .reg = &reg,
yihui 0:972f3778c19c 523 .hw = &hw,
yihui 0:972f3778c19c 524 .test = &test
yihui 0:972f3778c19c 525 };
yihui 0:972f3778c19c 526 #elif defined MPU6500
yihui 0:972f3778c19c 527 const struct gyro_reg_s reg = {
yihui 0:972f3778c19c 528 .who_am_i = 0x75,
yihui 0:972f3778c19c 529 .rate_div = 0x19,
yihui 0:972f3778c19c 530 .lpf = 0x1A,
yihui 0:972f3778c19c 531 .prod_id = 0x0C,
yihui 0:972f3778c19c 532 .user_ctrl = 0x6A,
yihui 0:972f3778c19c 533 .fifo_en = 0x23,
yihui 0:972f3778c19c 534 .gyro_cfg = 0x1B,
yihui 0:972f3778c19c 535 .accel_cfg = 0x1C,
yihui 0:972f3778c19c 536 .accel_cfg2 = 0x1D,
yihui 0:972f3778c19c 537 .lp_accel_odr = 0x1E,
yihui 0:972f3778c19c 538 .motion_thr = 0x1F,
yihui 0:972f3778c19c 539 .motion_dur = 0x20,
yihui 0:972f3778c19c 540 .fifo_count_h = 0x72,
yihui 0:972f3778c19c 541 .fifo_r_w = 0x74,
yihui 0:972f3778c19c 542 .raw_gyro = 0x43,
yihui 0:972f3778c19c 543 .raw_accel = 0x3B,
yihui 0:972f3778c19c 544 .temp = 0x41,
yihui 0:972f3778c19c 545 .int_enable = 0x38,
yihui 0:972f3778c19c 546 .dmp_int_status = 0x39,
yihui 0:972f3778c19c 547 .int_status = 0x3A,
yihui 0:972f3778c19c 548 .accel_intel = 0x69,
yihui 0:972f3778c19c 549 .pwr_mgmt_1 = 0x6B,
yihui 0:972f3778c19c 550 .pwr_mgmt_2 = 0x6C,
yihui 0:972f3778c19c 551 .int_pin_cfg = 0x37,
yihui 0:972f3778c19c 552 .mem_r_w = 0x6F,
yihui 0:972f3778c19c 553 .accel_offs = 0x77,
yihui 0:972f3778c19c 554 .i2c_mst = 0x24,
yihui 0:972f3778c19c 555 .bank_sel = 0x6D,
yihui 0:972f3778c19c 556 .mem_start_addr = 0x6E,
yihui 0:972f3778c19c 557 .prgm_start_h = 0x70
yihui 0:972f3778c19c 558 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 559 ,.raw_compass = 0x49,
yihui 0:972f3778c19c 560 .s0_addr = 0x25,
yihui 0:972f3778c19c 561 .s0_reg = 0x26,
yihui 0:972f3778c19c 562 .s0_ctrl = 0x27,
yihui 0:972f3778c19c 563 .s1_addr = 0x28,
yihui 0:972f3778c19c 564 .s1_reg = 0x29,
yihui 0:972f3778c19c 565 .s1_ctrl = 0x2A,
yihui 0:972f3778c19c 566 .s4_ctrl = 0x34,
yihui 0:972f3778c19c 567 .s0_do = 0x63,
yihui 0:972f3778c19c 568 .s1_do = 0x64,
yihui 0:972f3778c19c 569 .i2c_delay_ctrl = 0x67
yihui 0:972f3778c19c 570 #endif
yihui 0:972f3778c19c 571 };
yihui 0:972f3778c19c 572 const struct hw_s hw = {
yihui 0:972f3778c19c 573 .addr = 0x68,
yihui 0:972f3778c19c 574 .max_fifo = 1024,
yihui 0:972f3778c19c 575 .num_reg = 128,
yihui 0:972f3778c19c 576 .temp_sens = 321,
yihui 0:972f3778c19c 577 .temp_offset = 0,
yihui 0:972f3778c19c 578 .bank_size = 256
yihui 0:972f3778c19c 579 #if defined AK89xx_SECONDARY
yihui 0:972f3778c19c 580 ,.compass_fsr = AK89xx_FSR
yihui 0:972f3778c19c 581 #endif
yihui 0:972f3778c19c 582 };
yihui 0:972f3778c19c 583
yihui 0:972f3778c19c 584 const struct test_s test = {
yihui 0:972f3778c19c 585 .gyro_sens = 32768/250,
yihui 0:972f3778c19c 586 .accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G
yihui 0:972f3778c19c 587 .reg_rate_div = 0, /* 1kHz. */
yihui 0:972f3778c19c 588 .reg_lpf = 2, /* 92Hz low pass filter*/
yihui 0:972f3778c19c 589 .reg_gyro_fsr = 0, /* 250dps. */
yihui 0:972f3778c19c 590 .reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */
yihui 0:972f3778c19c 591 .wait_ms = 200, //200ms stabilization time
yihui 0:972f3778c19c 592 .packet_thresh = 200, /* 200 samples */
yihui 0:972f3778c19c 593 .min_dps = 20.f, //20 dps for Gyro Criteria C
yihui 0:972f3778c19c 594 .max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B
yihui 0:972f3778c19c 595 .max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A
yihui 0:972f3778c19c 596 .min_g = .225f, //Accel must exceed Min 225 mg for Criteria B
yihui 0:972f3778c19c 597 .max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B
yihui 0:972f3778c19c 598 .max_accel_var = .5f, //Accel must be within 50% variation for Criteria A
yihui 0:972f3778c19c 599 .max_g_offset = .5f, //500 mg for Accel Criteria C
yihui 0:972f3778c19c 600 .sample_wait_ms = 10 //10ms sample time wait
yihui 0:972f3778c19c 601 };
yihui 0:972f3778c19c 602
yihui 0:972f3778c19c 603 static struct gyro_state_s st = {
yihui 0:972f3778c19c 604 .reg = &reg,
yihui 0:972f3778c19c 605 .hw = &hw,
yihui 0:972f3778c19c 606 .test = &test
yihui 0:972f3778c19c 607 };
yihui 0:972f3778c19c 608 #endif
yihui 0:972f3778c19c 609
yihui 0:972f3778c19c 610 #define MAX_PACKET_LENGTH (12)
yihui 0:972f3778c19c 611 #ifdef MPU6500
yihui 0:972f3778c19c 612 #define HWST_MAX_PACKET_LENGTH (512)
yihui 0:972f3778c19c 613 #endif
yihui 0:972f3778c19c 614
yihui 0:972f3778c19c 615 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 616 static int setup_compass(void);
yihui 0:972f3778c19c 617 #define MAX_COMPASS_SAMPLE_RATE (100)
yihui 0:972f3778c19c 618 #endif
yihui 0:972f3778c19c 619
yihui 0:972f3778c19c 620 /**
yihui 0:972f3778c19c 621 * @brief Enable/disable data ready interrupt.
yihui 0:972f3778c19c 622 * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
yihui 0:972f3778c19c 623 * interrupt is used.
yihui 0:972f3778c19c 624 * @param[in] enable 1 to enable interrupt.
yihui 0:972f3778c19c 625 * @return 0 if successful.
yihui 0:972f3778c19c 626 */
yihui 0:972f3778c19c 627 static int set_int_enable(unsigned char enable)
yihui 0:972f3778c19c 628 {
yihui 0:972f3778c19c 629 unsigned char tmp;
yihui 0:972f3778c19c 630
yihui 0:972f3778c19c 631 if (st.chip_cfg.dmp_on) {
yihui 0:972f3778c19c 632 if (enable)
yihui 0:972f3778c19c 633 tmp = BIT_DMP_INT_EN;
yihui 0:972f3778c19c 634 else
yihui 0:972f3778c19c 635 tmp = 0x00;
yihui 0:972f3778c19c 636 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
yihui 0:972f3778c19c 637 return -1;
yihui 0:972f3778c19c 638 st.chip_cfg.int_enable = tmp;
yihui 0:972f3778c19c 639 } else {
yihui 0:972f3778c19c 640 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 641 return -1;
yihui 0:972f3778c19c 642 if (enable && st.chip_cfg.int_enable)
yihui 0:972f3778c19c 643 return 0;
yihui 0:972f3778c19c 644 if (enable)
yihui 0:972f3778c19c 645 tmp = BIT_DATA_RDY_EN;
yihui 0:972f3778c19c 646 else
yihui 0:972f3778c19c 647 tmp = 0x00;
yihui 0:972f3778c19c 648 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
yihui 0:972f3778c19c 649 return -1;
yihui 0:972f3778c19c 650 st.chip_cfg.int_enable = tmp;
yihui 0:972f3778c19c 651 }
yihui 0:972f3778c19c 652 return 0;
yihui 0:972f3778c19c 653 }
yihui 0:972f3778c19c 654
yihui 0:972f3778c19c 655 /**
yihui 0:972f3778c19c 656 * @brief Register dump for testing.
yihui 0:972f3778c19c 657 * @return 0 if successful.
yihui 0:972f3778c19c 658 */
yihui 0:972f3778c19c 659 int mpu_reg_dump(void)
yihui 0:972f3778c19c 660 {
yihui 0:972f3778c19c 661 unsigned char ii;
yihui 0:972f3778c19c 662 unsigned char data;
yihui 0:972f3778c19c 663
yihui 0:972f3778c19c 664 for (ii = 0; ii < st.hw->num_reg; ii++) {
yihui 0:972f3778c19c 665 if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
yihui 0:972f3778c19c 666 continue;
yihui 0:972f3778c19c 667 if (mpu_hal_read(st.hw->addr, ii, 1, &data))
yihui 0:972f3778c19c 668 return -1;
yihui 0:972f3778c19c 669 log_i("%#5x: %#5x\r\n", ii, data);
yihui 0:972f3778c19c 670 }
yihui 0:972f3778c19c 671 return 0;
yihui 0:972f3778c19c 672 }
yihui 0:972f3778c19c 673
yihui 0:972f3778c19c 674 /**
yihui 0:972f3778c19c 675 * @brief Read from a single register.
yihui 0:972f3778c19c 676 * NOTE: The memory and FIFO read/write registers cannot be accessed.
yihui 0:972f3778c19c 677 * @param[in] reg Register address.
yihui 0:972f3778c19c 678 * @param[out] data Register data.
yihui 0:972f3778c19c 679 * @return 0 if successful.
yihui 0:972f3778c19c 680 */
yihui 0:972f3778c19c 681 int mpu_read_reg(unsigned char reg, unsigned char *data)
yihui 0:972f3778c19c 682 {
yihui 0:972f3778c19c 683 if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
yihui 0:972f3778c19c 684 return -1;
yihui 0:972f3778c19c 685 if (reg >= st.hw->num_reg)
yihui 0:972f3778c19c 686 return -1;
yihui 0:972f3778c19c 687 return mpu_hal_read(st.hw->addr, reg, 1, data);
yihui 0:972f3778c19c 688 }
yihui 0:972f3778c19c 689
yihui 0:972f3778c19c 690 /**
yihui 0:972f3778c19c 691 * @brief Initialize hardware.
yihui 0:972f3778c19c 692 * Initial configuration:\n
yihui 0:972f3778c19c 693 * Gyro FSR: +/- 2000DPS\n
yihui 0:972f3778c19c 694 * Accel FSR +/- 2G\n
yihui 0:972f3778c19c 695 * DLPF: 42Hz\n
yihui 0:972f3778c19c 696 * FIFO rate: 50Hz\n
yihui 0:972f3778c19c 697 * Clock source: Gyro PLL\n
yihui 0:972f3778c19c 698 * FIFO: Disabled.\n
yihui 0:972f3778c19c 699 * Data ready interrupt: Disabled, active low, unlatched.
yihui 0:972f3778c19c 700 * @param[in] int_param Platform-specific parameters to interrupt API.
yihui 0:972f3778c19c 701 * @return 0 if successful.
yihui 0:972f3778c19c 702 */
yihui 0:972f3778c19c 703 int mpu_init(struct int_param_s *int_param)
yihui 0:972f3778c19c 704 {
yihui 0:972f3778c19c 705 unsigned char data[6];
yihui 0:972f3778c19c 706
yihui 0:972f3778c19c 707 /* Reset device. */
yihui 0:972f3778c19c 708 data[0] = BIT_RESET;
yihui 0:972f3778c19c 709 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
yihui 0:972f3778c19c 710 return -1;
yihui 0:972f3778c19c 711 delay_ms(100);
yihui 0:972f3778c19c 712
yihui 0:972f3778c19c 713 /* Wake up chip. */
yihui 0:972f3778c19c 714 data[0] = 0x00;
yihui 0:972f3778c19c 715 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
yihui 0:972f3778c19c 716 return -1;
yihui 0:972f3778c19c 717
yihui 0:972f3778c19c 718 st.chip_cfg.accel_half = 0;
yihui 0:972f3778c19c 719
yihui 0:972f3778c19c 720 #ifdef MPU6500
yihui 0:972f3778c19c 721 /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
yihui 0:972f3778c19c 722 * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
yihui 0:972f3778c19c 723 */
yihui 0:972f3778c19c 724 data[0] = BIT_FIFO_SIZE_1024 | 0x8;
yihui 0:972f3778c19c 725 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
yihui 0:972f3778c19c 726 return -1;
yihui 0:972f3778c19c 727 #endif
yihui 0:972f3778c19c 728
yihui 0:972f3778c19c 729 /* Set to invalid values to ensure no I2C writes are skipped. */
yihui 0:972f3778c19c 730 st.chip_cfg.sensors = 0xFF;
yihui 0:972f3778c19c 731 st.chip_cfg.gyro_fsr = 0xFF;
yihui 0:972f3778c19c 732 st.chip_cfg.accel_fsr = 0xFF;
yihui 0:972f3778c19c 733 st.chip_cfg.lpf = 0xFF;
yihui 0:972f3778c19c 734 st.chip_cfg.sample_rate = 0xFFFF;
yihui 0:972f3778c19c 735 st.chip_cfg.fifo_enable = 0xFF;
yihui 0:972f3778c19c 736 st.chip_cfg.bypass_mode = 0xFF;
yihui 0:972f3778c19c 737 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 738 st.chip_cfg.compass_sample_rate = 0xFFFF;
yihui 0:972f3778c19c 739 #endif
yihui 0:972f3778c19c 740 /* mpu_set_sensors always preserves this setting. */
yihui 0:972f3778c19c 741 st.chip_cfg.clk_src = INV_CLK_PLL;
yihui 0:972f3778c19c 742 /* Handled in next call to mpu_set_bypass. */
yihui 0:972f3778c19c 743 st.chip_cfg.active_low_int = 1;
yihui 0:972f3778c19c 744 st.chip_cfg.latched_int = 0;
yihui 0:972f3778c19c 745 st.chip_cfg.int_motion_only = 0;
yihui 0:972f3778c19c 746 st.chip_cfg.lp_accel_mode = 0;
yihui 0:972f3778c19c 747 memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
yihui 0:972f3778c19c 748 st.chip_cfg.dmp_on = 0;
yihui 0:972f3778c19c 749 st.chip_cfg.dmp_loaded = 0;
yihui 0:972f3778c19c 750 st.chip_cfg.dmp_sample_rate = 0;
yihui 0:972f3778c19c 751
yihui 0:972f3778c19c 752 if (mpu_set_gyro_fsr(2000))
yihui 0:972f3778c19c 753 return -1;
yihui 0:972f3778c19c 754 if (mpu_set_accel_fsr(2))
yihui 0:972f3778c19c 755 return -1;
yihui 0:972f3778c19c 756 if (mpu_set_lpf(42))
yihui 0:972f3778c19c 757 return -1;
yihui 0:972f3778c19c 758 if (mpu_set_sample_rate(50))
yihui 0:972f3778c19c 759 return -1;
yihui 0:972f3778c19c 760 if (mpu_configure_fifo(0))
yihui 0:972f3778c19c 761 return -1;
yihui 0:972f3778c19c 762
yihui 0:972f3778c19c 763 if (int_param)
yihui 0:972f3778c19c 764 reg_int_cb(int_param);
yihui 0:972f3778c19c 765
yihui 0:972f3778c19c 766 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 767 setup_compass();
yihui 0:972f3778c19c 768 if (mpu_set_compass_sample_rate(10))
yihui 0:972f3778c19c 769 return -1;
yihui 0:972f3778c19c 770 #else
yihui 0:972f3778c19c 771 /* Already disabled by setup_compass. */
yihui 0:972f3778c19c 772 if (mpu_set_bypass(0))
yihui 0:972f3778c19c 773 return -1;
yihui 0:972f3778c19c 774 #endif
yihui 0:972f3778c19c 775
yihui 0:972f3778c19c 776 mpu_set_sensors(0);
yihui 0:972f3778c19c 777 return 0;
yihui 0:972f3778c19c 778 }
yihui 0:972f3778c19c 779
yihui 0:972f3778c19c 780 /**
yihui 0:972f3778c19c 781 * @brief Enter low-power accel-only mode.
yihui 0:972f3778c19c 782 * In low-power accel mode, the chip goes to sleep and only wakes up to sample
yihui 0:972f3778c19c 783 * the accelerometer at one of the following frequencies:
yihui 0:972f3778c19c 784 * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
yihui 0:972f3778c19c 785 * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
yihui 0:972f3778c19c 786 * \n If the requested rate is not one listed above, the device will be set to
yihui 0:972f3778c19c 787 * the next highest rate. Requesting a rate above the maximum supported
yihui 0:972f3778c19c 788 * frequency will result in an error.
yihui 0:972f3778c19c 789 * \n To select a fractional wake-up frequency, round down the value passed to
yihui 0:972f3778c19c 790 * @e rate.
yihui 0:972f3778c19c 791 * @param[in] rate Minimum sampling rate, or zero to disable LP
yihui 0:972f3778c19c 792 * accel mode.
yihui 0:972f3778c19c 793 * @return 0 if successful.
yihui 0:972f3778c19c 794 */
yihui 0:972f3778c19c 795 int mpu_lp_accel_mode(unsigned char rate)
yihui 0:972f3778c19c 796 {
yihui 0:972f3778c19c 797 unsigned char tmp[2];
yihui 0:972f3778c19c 798
yihui 0:972f3778c19c 799 if (rate > 40)
yihui 0:972f3778c19c 800 return -1;
yihui 0:972f3778c19c 801
yihui 0:972f3778c19c 802 if (!rate) {
yihui 0:972f3778c19c 803 mpu_set_int_latched(0);
yihui 0:972f3778c19c 804 tmp[0] = 0;
yihui 0:972f3778c19c 805 tmp[1] = BIT_STBY_XYZG;
yihui 0:972f3778c19c 806 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
yihui 0:972f3778c19c 807 return -1;
yihui 0:972f3778c19c 808 st.chip_cfg.lp_accel_mode = 0;
yihui 0:972f3778c19c 809 return 0;
yihui 0:972f3778c19c 810 }
yihui 0:972f3778c19c 811 /* For LP accel, we automatically configure the hardware to produce latched
yihui 0:972f3778c19c 812 * interrupts. In LP accel mode, the hardware cycles into sleep mode before
yihui 0:972f3778c19c 813 * it gets a chance to deassert the interrupt pin; therefore, we shift this
yihui 0:972f3778c19c 814 * responsibility over to the MCU.
yihui 0:972f3778c19c 815 *
yihui 0:972f3778c19c 816 * Any register read will clear the interrupt.
yihui 0:972f3778c19c 817 */
yihui 0:972f3778c19c 818 mpu_set_int_latched(1);
yihui 0:972f3778c19c 819 #if defined MPU6050
yihui 0:972f3778c19c 820 tmp[0] = BIT_LPA_CYCLE;
yihui 0:972f3778c19c 821 if (rate == 1) {
yihui 0:972f3778c19c 822 tmp[1] = INV_LPA_1_25HZ;
yihui 0:972f3778c19c 823 mpu_set_lpf(5);
yihui 0:972f3778c19c 824 } else if (rate <= 5) {
yihui 0:972f3778c19c 825 tmp[1] = INV_LPA_5HZ;
yihui 0:972f3778c19c 826 mpu_set_lpf(5);
yihui 0:972f3778c19c 827 } else if (rate <= 20) {
yihui 0:972f3778c19c 828 tmp[1] = INV_LPA_20HZ;
yihui 0:972f3778c19c 829 mpu_set_lpf(10);
yihui 0:972f3778c19c 830 } else {
yihui 0:972f3778c19c 831 tmp[1] = INV_LPA_40HZ;
yihui 0:972f3778c19c 832 mpu_set_lpf(20);
yihui 0:972f3778c19c 833 }
yihui 0:972f3778c19c 834 tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
yihui 0:972f3778c19c 835 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
yihui 0:972f3778c19c 836 return -1;
yihui 0:972f3778c19c 837 #elif defined MPU6500
yihui 0:972f3778c19c 838 /* Set wake frequency. */
yihui 0:972f3778c19c 839 if (rate == 1)
yihui 0:972f3778c19c 840 tmp[0] = INV_LPA_1_25HZ;
yihui 0:972f3778c19c 841 else if (rate == 2)
yihui 0:972f3778c19c 842 tmp[0] = INV_LPA_2_5HZ;
yihui 0:972f3778c19c 843 else if (rate <= 5)
yihui 0:972f3778c19c 844 tmp[0] = INV_LPA_5HZ;
yihui 0:972f3778c19c 845 else if (rate <= 10)
yihui 0:972f3778c19c 846 tmp[0] = INV_LPA_10HZ;
yihui 0:972f3778c19c 847 else if (rate <= 20)
yihui 0:972f3778c19c 848 tmp[0] = INV_LPA_20HZ;
yihui 0:972f3778c19c 849 else if (rate <= 40)
yihui 0:972f3778c19c 850 tmp[0] = INV_LPA_40HZ;
yihui 0:972f3778c19c 851 else if (rate <= 80)
yihui 0:972f3778c19c 852 tmp[0] = INV_LPA_80HZ;
yihui 0:972f3778c19c 853 else if (rate <= 160)
yihui 0:972f3778c19c 854 tmp[0] = INV_LPA_160HZ;
yihui 0:972f3778c19c 855 else if (rate <= 320)
yihui 0:972f3778c19c 856 tmp[0] = INV_LPA_320HZ;
yihui 0:972f3778c19c 857 else
yihui 0:972f3778c19c 858 tmp[0] = INV_LPA_640HZ;
yihui 0:972f3778c19c 859 if (mpu_hal_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
yihui 0:972f3778c19c 860 return -1;
yihui 0:972f3778c19c 861 tmp[0] = BIT_LPA_CYCLE;
yihui 0:972f3778c19c 862 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
yihui 0:972f3778c19c 863 return -1;
yihui 0:972f3778c19c 864 #endif
yihui 0:972f3778c19c 865 st.chip_cfg.sensors = INV_XYZ_ACCEL;
yihui 0:972f3778c19c 866 st.chip_cfg.clk_src = 0;
yihui 0:972f3778c19c 867 st.chip_cfg.lp_accel_mode = 1;
yihui 0:972f3778c19c 868 mpu_configure_fifo(0);
yihui 0:972f3778c19c 869
yihui 0:972f3778c19c 870 return 0;
yihui 0:972f3778c19c 871 }
yihui 0:972f3778c19c 872
yihui 0:972f3778c19c 873 /**
yihui 0:972f3778c19c 874 * @brief Read raw gyro data directly from the registers.
yihui 0:972f3778c19c 875 * @param[out] data Raw data in hardware units.
yihui 0:972f3778c19c 876 * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
yihui 0:972f3778c19c 877 * @return 0 if successful.
yihui 0:972f3778c19c 878 */
yihui 0:972f3778c19c 879 int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
yihui 0:972f3778c19c 880 {
yihui 0:972f3778c19c 881 unsigned char tmp[6];
yihui 0:972f3778c19c 882
yihui 0:972f3778c19c 883 if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
yihui 0:972f3778c19c 884 return -1;
yihui 0:972f3778c19c 885
yihui 0:972f3778c19c 886 if (mpu_hal_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
yihui 0:972f3778c19c 887 return -1;
yihui 0:972f3778c19c 888 data[0] = (tmp[0] << 8) | tmp[1];
yihui 0:972f3778c19c 889 data[1] = (tmp[2] << 8) | tmp[3];
yihui 0:972f3778c19c 890 data[2] = (tmp[4] << 8) | tmp[5];
yihui 0:972f3778c19c 891 if (timestamp)
yihui 0:972f3778c19c 892 get_ms(timestamp);
yihui 0:972f3778c19c 893 return 0;
yihui 0:972f3778c19c 894 }
yihui 0:972f3778c19c 895
yihui 0:972f3778c19c 896 /**
yihui 0:972f3778c19c 897 * @brief Read raw accel data directly from the registers.
yihui 0:972f3778c19c 898 * @param[out] data Raw data in hardware units.
yihui 0:972f3778c19c 899 * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
yihui 0:972f3778c19c 900 * @return 0 if successful.
yihui 0:972f3778c19c 901 */
yihui 0:972f3778c19c 902 int mpu_get_accel_reg(short *data, unsigned long *timestamp)
yihui 0:972f3778c19c 903 {
yihui 0:972f3778c19c 904 unsigned char tmp[6];
yihui 0:972f3778c19c 905
yihui 0:972f3778c19c 906 if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
yihui 0:972f3778c19c 907 return -1;
yihui 0:972f3778c19c 908
yihui 0:972f3778c19c 909 if (mpu_hal_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
yihui 0:972f3778c19c 910 return -1;
yihui 0:972f3778c19c 911 data[0] = (tmp[0] << 8) | tmp[1];
yihui 0:972f3778c19c 912 data[1] = (tmp[2] << 8) | tmp[3];
yihui 0:972f3778c19c 913 data[2] = (tmp[4] << 8) | tmp[5];
yihui 0:972f3778c19c 914 if (timestamp)
yihui 0:972f3778c19c 915 get_ms(timestamp);
yihui 0:972f3778c19c 916 return 0;
yihui 0:972f3778c19c 917 }
yihui 0:972f3778c19c 918
yihui 0:972f3778c19c 919 /**
yihui 0:972f3778c19c 920 * @brief Read temperature data directly from the registers.
yihui 0:972f3778c19c 921 * @param[out] data Data in q16 format.
yihui 0:972f3778c19c 922 * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
yihui 0:972f3778c19c 923 * @return 0 if successful.
yihui 0:972f3778c19c 924 */
yihui 0:972f3778c19c 925 int mpu_get_temperature(long *data, unsigned long *timestamp)
yihui 0:972f3778c19c 926 {
yihui 0:972f3778c19c 927 unsigned char tmp[2];
yihui 0:972f3778c19c 928 short raw;
yihui 0:972f3778c19c 929
yihui 0:972f3778c19c 930 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 931 return -1;
yihui 0:972f3778c19c 932
yihui 0:972f3778c19c 933 if (mpu_hal_read(st.hw->addr, st.reg->temp, 2, tmp))
yihui 0:972f3778c19c 934 return -1;
yihui 0:972f3778c19c 935 raw = (tmp[0] << 8) | tmp[1];
yihui 0:972f3778c19c 936 if (timestamp)
yihui 0:972f3778c19c 937 get_ms(timestamp);
yihui 0:972f3778c19c 938
yihui 0:972f3778c19c 939 data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
yihui 0:972f3778c19c 940 return 0;
yihui 0:972f3778c19c 941 }
yihui 0:972f3778c19c 942
yihui 0:972f3778c19c 943 /**
yihui 0:972f3778c19c 944 * @brief Read biases to the accel bias 6500 registers.
yihui 0:972f3778c19c 945 * This function reads from the MPU6500 accel offset cancellations registers.
yihui 0:972f3778c19c 946 * The format are G in +-8G format. The register is initialized with OTP
yihui 0:972f3778c19c 947 * factory trim values.
yihui 0:972f3778c19c 948 * @param[in] accel_bias returned structure with the accel bias
yihui 0:972f3778c19c 949 * @return 0 if successful.
yihui 0:972f3778c19c 950 */
yihui 0:972f3778c19c 951 int mpu_read_6500_accel_bias(long *accel_bias) {
yihui 0:972f3778c19c 952 unsigned char data[6];
yihui 0:972f3778c19c 953 if (mpu_hal_read(st.hw->addr, 0x77, 2, &data[0]))
yihui 0:972f3778c19c 954 return -1;
yihui 0:972f3778c19c 955 if (mpu_hal_read(st.hw->addr, 0x7A, 2, &data[2]))
yihui 0:972f3778c19c 956 return -1;
yihui 0:972f3778c19c 957 if (mpu_hal_read(st.hw->addr, 0x7D, 2, &data[4]))
yihui 0:972f3778c19c 958 return -1;
yihui 0:972f3778c19c 959 accel_bias[0] = ((long)data[0]<<8) | data[1];
yihui 0:972f3778c19c 960 accel_bias[1] = ((long)data[2]<<8) | data[3];
yihui 0:972f3778c19c 961 accel_bias[2] = ((long)data[4]<<8) | data[5];
yihui 0:972f3778c19c 962 return 0;
yihui 0:972f3778c19c 963 }
yihui 0:972f3778c19c 964
yihui 0:972f3778c19c 965 /**
yihui 0:972f3778c19c 966 * @brief Read biases to the accel bias 6050 registers.
yihui 0:972f3778c19c 967 * This function reads from the MPU6050 accel offset cancellations registers.
yihui 0:972f3778c19c 968 * The format are G in +-8G format. The register is initialized with OTP
yihui 0:972f3778c19c 969 * factory trim values.
yihui 0:972f3778c19c 970 * @param[in] accel_bias returned structure with the accel bias
yihui 0:972f3778c19c 971 * @return 0 if successful.
yihui 0:972f3778c19c 972 */
yihui 0:972f3778c19c 973 int mpu_read_6050_accel_bias(long *accel_bias) {
yihui 0:972f3778c19c 974 unsigned char data[6];
yihui 0:972f3778c19c 975 if (mpu_hal_read(st.hw->addr, 0x06, 2, &data[0]))
yihui 0:972f3778c19c 976 return -1;
yihui 0:972f3778c19c 977 if (mpu_hal_read(st.hw->addr, 0x08, 2, &data[2]))
yihui 0:972f3778c19c 978 return -1;
yihui 0:972f3778c19c 979 if (mpu_hal_read(st.hw->addr, 0x0A, 2, &data[4]))
yihui 0:972f3778c19c 980 return -1;
yihui 0:972f3778c19c 981 accel_bias[0] = ((long)data[0]<<8) | data[1];
yihui 0:972f3778c19c 982 accel_bias[1] = ((long)data[2]<<8) | data[3];
yihui 0:972f3778c19c 983 accel_bias[2] = ((long)data[4]<<8) | data[5];
yihui 0:972f3778c19c 984 return 0;
yihui 0:972f3778c19c 985 }
yihui 0:972f3778c19c 986
yihui 0:972f3778c19c 987 int mpu_read_6500_gyro_bias(long *gyro_bias) {
yihui 0:972f3778c19c 988 unsigned char data[6];
yihui 0:972f3778c19c 989 if (mpu_hal_read(st.hw->addr, 0x13, 2, &data[0]))
yihui 0:972f3778c19c 990 return -1;
yihui 0:972f3778c19c 991 if (mpu_hal_read(st.hw->addr, 0x15, 2, &data[2]))
yihui 0:972f3778c19c 992 return -1;
yihui 0:972f3778c19c 993 if (mpu_hal_read(st.hw->addr, 0x17, 2, &data[4]))
yihui 0:972f3778c19c 994 return -1;
yihui 0:972f3778c19c 995 gyro_bias[0] = ((long)data[0]<<8) | data[1];
yihui 0:972f3778c19c 996 gyro_bias[1] = ((long)data[2]<<8) | data[3];
yihui 0:972f3778c19c 997 gyro_bias[2] = ((long)data[4]<<8) | data[5];
yihui 0:972f3778c19c 998 return 0;
yihui 0:972f3778c19c 999 }
yihui 0:972f3778c19c 1000
yihui 0:972f3778c19c 1001 /**
yihui 0:972f3778c19c 1002 * @brief Push biases to the gyro bias 6500/6050 registers.
yihui 0:972f3778c19c 1003 * This function expects biases relative to the current sensor output, and
yihui 0:972f3778c19c 1004 * these biases will be added to the factory-supplied values. Bias inputs are LSB
yihui 0:972f3778c19c 1005 * in +-1000dps format.
yihui 0:972f3778c19c 1006 * @param[in] gyro_bias New biases.
yihui 0:972f3778c19c 1007 * @return 0 if successful.
yihui 0:972f3778c19c 1008 */
yihui 0:972f3778c19c 1009 int mpu_set_gyro_bias_reg(long *gyro_bias)
yihui 0:972f3778c19c 1010 {
yihui 0:972f3778c19c 1011 unsigned char data[6] = {0, 0, 0, 0, 0, 0};
yihui 0:972f3778c19c 1012 int i=0;
yihui 0:972f3778c19c 1013 for(i=0;i<3;i++) {
yihui 0:972f3778c19c 1014 gyro_bias[i]= (-gyro_bias[i]);
yihui 0:972f3778c19c 1015 }
yihui 0:972f3778c19c 1016 data[0] = (gyro_bias[0] >> 8) & 0xff;
yihui 0:972f3778c19c 1017 data[1] = (gyro_bias[0]) & 0xff;
yihui 0:972f3778c19c 1018 data[2] = (gyro_bias[1] >> 8) & 0xff;
yihui 0:972f3778c19c 1019 data[3] = (gyro_bias[1]) & 0xff;
yihui 0:972f3778c19c 1020 data[4] = (gyro_bias[2] >> 8) & 0xff;
yihui 0:972f3778c19c 1021 data[5] = (gyro_bias[2]) & 0xff;
yihui 0:972f3778c19c 1022 if (mpu_hal_write(st.hw->addr, 0x13, 2, &data[0]))
yihui 0:972f3778c19c 1023 return -1;
yihui 0:972f3778c19c 1024 if (mpu_hal_write(st.hw->addr, 0x15, 2, &data[2]))
yihui 0:972f3778c19c 1025 return -1;
yihui 0:972f3778c19c 1026 if (mpu_hal_write(st.hw->addr, 0x17, 2, &data[4]))
yihui 0:972f3778c19c 1027 return -1;
yihui 0:972f3778c19c 1028 return 0;
yihui 0:972f3778c19c 1029 }
yihui 0:972f3778c19c 1030
yihui 0:972f3778c19c 1031 /**
yihui 0:972f3778c19c 1032 * @brief Push biases to the accel bias 6050 registers.
yihui 0:972f3778c19c 1033 * This function expects biases relative to the current sensor output, and
yihui 0:972f3778c19c 1034 * these biases will be added to the factory-supplied values. Bias inputs are LSB
yihui 0:972f3778c19c 1035 * in +-8G format.
yihui 0:972f3778c19c 1036 * @param[in] accel_bias New biases.
yihui 0:972f3778c19c 1037 * @return 0 if successful.
yihui 0:972f3778c19c 1038 */
yihui 0:972f3778c19c 1039 int mpu_set_accel_bias_6050_reg(const long *accel_bias)
yihui 0:972f3778c19c 1040 {
yihui 0:972f3778c19c 1041 unsigned char data[6] = {0, 0, 0, 0, 0, 0};
yihui 0:972f3778c19c 1042 long accel_reg_bias[3] = {0, 0, 0};
yihui 0:972f3778c19c 1043 long mask = 0x0001;
yihui 0:972f3778c19c 1044 unsigned char mask_bit[3] = {0, 0, 0};
yihui 0:972f3778c19c 1045 unsigned char i = 0;
yihui 0:972f3778c19c 1046 if(mpu_read_6050_accel_bias(accel_reg_bias))
yihui 0:972f3778c19c 1047 return -1;
yihui 0:972f3778c19c 1048
yihui 0:972f3778c19c 1049 //bit 0 of the 2 byte bias is for temp comp
yihui 0:972f3778c19c 1050 //calculations need to compensate for this and not change it
yihui 0:972f3778c19c 1051 for(i=0; i<3; i++) {
yihui 0:972f3778c19c 1052 if(accel_reg_bias[i]&mask)
yihui 0:972f3778c19c 1053 mask_bit[i] = 0x01;
yihui 0:972f3778c19c 1054 }
yihui 0:972f3778c19c 1055
yihui 0:972f3778c19c 1056 accel_reg_bias[0] -= accel_bias[0];
yihui 0:972f3778c19c 1057 accel_reg_bias[1] -= accel_bias[1];
yihui 0:972f3778c19c 1058 accel_reg_bias[2] -= accel_bias[2];
yihui 0:972f3778c19c 1059
yihui 0:972f3778c19c 1060 data[0] = (accel_reg_bias[0] >> 8) & 0xff;
yihui 0:972f3778c19c 1061 data[1] = (accel_reg_bias[0]) & 0xff;
yihui 0:972f3778c19c 1062 data[1] = data[1]|mask_bit[0];
yihui 0:972f3778c19c 1063 data[2] = (accel_reg_bias[1] >> 8) & 0xff;
yihui 0:972f3778c19c 1064 data[3] = (accel_reg_bias[1]) & 0xff;
yihui 0:972f3778c19c 1065 data[3] = data[3]|mask_bit[1];
yihui 0:972f3778c19c 1066 data[4] = (accel_reg_bias[2] >> 8) & 0xff;
yihui 0:972f3778c19c 1067 data[5] = (accel_reg_bias[2]) & 0xff;
yihui 0:972f3778c19c 1068 data[5] = data[5]|mask_bit[2];
yihui 0:972f3778c19c 1069
yihui 0:972f3778c19c 1070 if (mpu_hal_write(st.hw->addr, 0x06, 2, &data[0]))
yihui 0:972f3778c19c 1071 return -1;
yihui 0:972f3778c19c 1072 if (mpu_hal_write(st.hw->addr, 0x08, 2, &data[2]))
yihui 0:972f3778c19c 1073 return -1;
yihui 0:972f3778c19c 1074 if (mpu_hal_write(st.hw->addr, 0x0A, 2, &data[4]))
yihui 0:972f3778c19c 1075 return -1;
yihui 0:972f3778c19c 1076
yihui 0:972f3778c19c 1077 return 0;
yihui 0:972f3778c19c 1078 }
yihui 0:972f3778c19c 1079
yihui 0:972f3778c19c 1080
yihui 0:972f3778c19c 1081 /**
yihui 0:972f3778c19c 1082 * @brief Push biases to the accel bias 6500 registers.
yihui 0:972f3778c19c 1083 * This function expects biases relative to the current sensor output, and
yihui 0:972f3778c19c 1084 * these biases will be added to the factory-supplied values. Bias inputs are LSB
yihui 0:972f3778c19c 1085 * in +-8G format.
yihui 0:972f3778c19c 1086 * @param[in] accel_bias New biases.
yihui 0:972f3778c19c 1087 * @return 0 if successful.
yihui 0:972f3778c19c 1088 */
yihui 0:972f3778c19c 1089 int mpu_set_accel_bias_6500_reg(const long *accel_bias)
yihui 0:972f3778c19c 1090 {
yihui 0:972f3778c19c 1091 unsigned char data[6] = {0, 0, 0, 0, 0, 0};
yihui 0:972f3778c19c 1092 long accel_reg_bias[3] = {0, 0, 0};
yihui 0:972f3778c19c 1093 long mask = 0x0001;
yihui 0:972f3778c19c 1094 unsigned char mask_bit[3] = {0, 0, 0};
yihui 0:972f3778c19c 1095 unsigned char i = 0;
yihui 0:972f3778c19c 1096
yihui 0:972f3778c19c 1097 if(mpu_read_6500_accel_bias(accel_reg_bias))
yihui 0:972f3778c19c 1098 return -1;
yihui 0:972f3778c19c 1099
yihui 0:972f3778c19c 1100 //bit 0 of the 2 byte bias is for temp comp
yihui 0:972f3778c19c 1101 //calculations need to compensate for this
yihui 0:972f3778c19c 1102 for(i=0; i<3; i++) {
yihui 0:972f3778c19c 1103 if(accel_reg_bias[i]&mask)
yihui 0:972f3778c19c 1104 mask_bit[i] = 0x01;
yihui 0:972f3778c19c 1105 }
yihui 0:972f3778c19c 1106
yihui 0:972f3778c19c 1107 accel_reg_bias[0] -= accel_bias[0];
yihui 0:972f3778c19c 1108 accel_reg_bias[1] -= accel_bias[1];
yihui 0:972f3778c19c 1109 accel_reg_bias[2] -= accel_bias[2];
yihui 0:972f3778c19c 1110
yihui 0:972f3778c19c 1111 data[0] = (accel_reg_bias[0] >> 8) & 0xff;
yihui 0:972f3778c19c 1112 data[1] = (accel_reg_bias[0]) & 0xff;
yihui 0:972f3778c19c 1113 data[1] = data[1]|mask_bit[0];
yihui 0:972f3778c19c 1114 data[2] = (accel_reg_bias[1] >> 8) & 0xff;
yihui 0:972f3778c19c 1115 data[3] = (accel_reg_bias[1]) & 0xff;
yihui 0:972f3778c19c 1116 data[3] = data[3]|mask_bit[1];
yihui 0:972f3778c19c 1117 data[4] = (accel_reg_bias[2] >> 8) & 0xff;
yihui 0:972f3778c19c 1118 data[5] = (accel_reg_bias[2]) & 0xff;
yihui 0:972f3778c19c 1119 data[5] = data[5]|mask_bit[2];
yihui 0:972f3778c19c 1120
yihui 0:972f3778c19c 1121 if (mpu_hal_write(st.hw->addr, 0x77, 2, &data[0]))
yihui 0:972f3778c19c 1122 return -1;
yihui 0:972f3778c19c 1123 if (mpu_hal_write(st.hw->addr, 0x7A, 2, &data[2]))
yihui 0:972f3778c19c 1124 return -1;
yihui 0:972f3778c19c 1125 if (mpu_hal_write(st.hw->addr, 0x7D, 2, &data[4]))
yihui 0:972f3778c19c 1126 return -1;
yihui 0:972f3778c19c 1127
yihui 0:972f3778c19c 1128 return 0;
yihui 0:972f3778c19c 1129 }
yihui 0:972f3778c19c 1130
yihui 0:972f3778c19c 1131 /**
yihui 0:972f3778c19c 1132 * @brief Reset FIFO read/write pointers.
yihui 0:972f3778c19c 1133 * @return 0 if successful.
yihui 0:972f3778c19c 1134 */
yihui 0:972f3778c19c 1135 int mpu_reset_fifo(void)
yihui 0:972f3778c19c 1136 {
yihui 0:972f3778c19c 1137 unsigned char data;
yihui 0:972f3778c19c 1138
yihui 0:972f3778c19c 1139 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1140 return -1;
yihui 0:972f3778c19c 1141
yihui 0:972f3778c19c 1142 data = 0;
yihui 0:972f3778c19c 1143 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data))
yihui 0:972f3778c19c 1144 return -1;
yihui 0:972f3778c19c 1145 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &data))
yihui 0:972f3778c19c 1146 return -1;
yihui 0:972f3778c19c 1147 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
yihui 0:972f3778c19c 1148 return -1;
yihui 0:972f3778c19c 1149
yihui 0:972f3778c19c 1150 if (st.chip_cfg.dmp_on) {
yihui 0:972f3778c19c 1151 data = BIT_FIFO_RST | BIT_DMP_RST;
yihui 0:972f3778c19c 1152 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
yihui 0:972f3778c19c 1153 return -1;
yihui 0:972f3778c19c 1154 delay_ms(50);
yihui 0:972f3778c19c 1155 data = BIT_DMP_EN | BIT_FIFO_EN;
yihui 0:972f3778c19c 1156 if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
yihui 0:972f3778c19c 1157 data |= BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1158 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
yihui 0:972f3778c19c 1159 return -1;
yihui 0:972f3778c19c 1160 if (st.chip_cfg.int_enable)
yihui 0:972f3778c19c 1161 data = BIT_DMP_INT_EN;
yihui 0:972f3778c19c 1162 else
yihui 0:972f3778c19c 1163 data = 0;
yihui 0:972f3778c19c 1164 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data))
yihui 0:972f3778c19c 1165 return -1;
yihui 0:972f3778c19c 1166 data = 0;
yihui 0:972f3778c19c 1167 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &data))
yihui 0:972f3778c19c 1168 return -1;
yihui 0:972f3778c19c 1169 } else {
yihui 0:972f3778c19c 1170 data = BIT_FIFO_RST;
yihui 0:972f3778c19c 1171 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
yihui 0:972f3778c19c 1172 return -1;
yihui 0:972f3778c19c 1173 if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
yihui 0:972f3778c19c 1174 data = BIT_FIFO_EN;
yihui 0:972f3778c19c 1175 else
yihui 0:972f3778c19c 1176 data = BIT_FIFO_EN | BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1177 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
yihui 0:972f3778c19c 1178 return -1;
yihui 0:972f3778c19c 1179 delay_ms(50);
yihui 0:972f3778c19c 1180 if (st.chip_cfg.int_enable)
yihui 0:972f3778c19c 1181 data = BIT_DATA_RDY_EN;
yihui 0:972f3778c19c 1182 else
yihui 0:972f3778c19c 1183 data = 0;
yihui 0:972f3778c19c 1184 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data))
yihui 0:972f3778c19c 1185 return -1;
yihui 0:972f3778c19c 1186 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
yihui 0:972f3778c19c 1187 return -1;
yihui 0:972f3778c19c 1188 }
yihui 0:972f3778c19c 1189 return 0;
yihui 0:972f3778c19c 1190 }
yihui 0:972f3778c19c 1191
yihui 0:972f3778c19c 1192 /**
yihui 0:972f3778c19c 1193 * @brief Get the gyro full-scale range.
yihui 0:972f3778c19c 1194 * @param[out] fsr Current full-scale range.
yihui 0:972f3778c19c 1195 * @return 0 if successful.
yihui 0:972f3778c19c 1196 */
yihui 0:972f3778c19c 1197 int mpu_get_gyro_fsr(unsigned short *fsr)
yihui 0:972f3778c19c 1198 {
yihui 0:972f3778c19c 1199 switch (st.chip_cfg.gyro_fsr) {
yihui 0:972f3778c19c 1200 case INV_FSR_250DPS:
yihui 0:972f3778c19c 1201 fsr[0] = 250;
yihui 0:972f3778c19c 1202 break;
yihui 0:972f3778c19c 1203 case INV_FSR_500DPS:
yihui 0:972f3778c19c 1204 fsr[0] = 500;
yihui 0:972f3778c19c 1205 break;
yihui 0:972f3778c19c 1206 case INV_FSR_1000DPS:
yihui 0:972f3778c19c 1207 fsr[0] = 1000;
yihui 0:972f3778c19c 1208 break;
yihui 0:972f3778c19c 1209 case INV_FSR_2000DPS:
yihui 0:972f3778c19c 1210 fsr[0] = 2000;
yihui 0:972f3778c19c 1211 break;
yihui 0:972f3778c19c 1212 default:
yihui 0:972f3778c19c 1213 fsr[0] = 0;
yihui 0:972f3778c19c 1214 break;
yihui 0:972f3778c19c 1215 }
yihui 0:972f3778c19c 1216 return 0;
yihui 0:972f3778c19c 1217 }
yihui 0:972f3778c19c 1218
yihui 0:972f3778c19c 1219 /**
yihui 0:972f3778c19c 1220 * @brief Set the gyro full-scale range.
yihui 0:972f3778c19c 1221 * @param[in] fsr Desired full-scale range.
yihui 0:972f3778c19c 1222 * @return 0 if successful.
yihui 0:972f3778c19c 1223 */
yihui 0:972f3778c19c 1224 int mpu_set_gyro_fsr(unsigned short fsr)
yihui 0:972f3778c19c 1225 {
yihui 0:972f3778c19c 1226 unsigned char data;
yihui 0:972f3778c19c 1227
yihui 0:972f3778c19c 1228 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1229 return -1;
yihui 0:972f3778c19c 1230
yihui 0:972f3778c19c 1231 switch (fsr) {
yihui 0:972f3778c19c 1232 case 250:
yihui 0:972f3778c19c 1233 data = INV_FSR_250DPS << 3;
yihui 0:972f3778c19c 1234 break;
yihui 0:972f3778c19c 1235 case 500:
yihui 0:972f3778c19c 1236 data = INV_FSR_500DPS << 3;
yihui 0:972f3778c19c 1237 break;
yihui 0:972f3778c19c 1238 case 1000:
yihui 0:972f3778c19c 1239 data = INV_FSR_1000DPS << 3;
yihui 0:972f3778c19c 1240 break;
yihui 0:972f3778c19c 1241 case 2000:
yihui 0:972f3778c19c 1242 data = INV_FSR_2000DPS << 3;
yihui 0:972f3778c19c 1243 break;
yihui 0:972f3778c19c 1244 default:
yihui 0:972f3778c19c 1245 return -1;
yihui 0:972f3778c19c 1246 }
yihui 0:972f3778c19c 1247
yihui 0:972f3778c19c 1248 if (st.chip_cfg.gyro_fsr == (data >> 3))
yihui 0:972f3778c19c 1249 return 0;
yihui 0:972f3778c19c 1250 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
yihui 0:972f3778c19c 1251 return -1;
yihui 0:972f3778c19c 1252 st.chip_cfg.gyro_fsr = data >> 3;
yihui 0:972f3778c19c 1253 return 0;
yihui 0:972f3778c19c 1254 }
yihui 0:972f3778c19c 1255
yihui 0:972f3778c19c 1256 /**
yihui 0:972f3778c19c 1257 * @brief Get the accel full-scale range.
yihui 0:972f3778c19c 1258 * @param[out] fsr Current full-scale range.
yihui 0:972f3778c19c 1259 * @return 0 if successful.
yihui 0:972f3778c19c 1260 */
yihui 0:972f3778c19c 1261 int mpu_get_accel_fsr(unsigned char *fsr)
yihui 0:972f3778c19c 1262 {
yihui 0:972f3778c19c 1263 switch (st.chip_cfg.accel_fsr) {
yihui 0:972f3778c19c 1264 case INV_FSR_2G:
yihui 0:972f3778c19c 1265 fsr[0] = 2;
yihui 0:972f3778c19c 1266 break;
yihui 0:972f3778c19c 1267 case INV_FSR_4G:
yihui 0:972f3778c19c 1268 fsr[0] = 4;
yihui 0:972f3778c19c 1269 break;
yihui 0:972f3778c19c 1270 case INV_FSR_8G:
yihui 0:972f3778c19c 1271 fsr[0] = 8;
yihui 0:972f3778c19c 1272 break;
yihui 0:972f3778c19c 1273 case INV_FSR_16G:
yihui 0:972f3778c19c 1274 fsr[0] = 16;
yihui 0:972f3778c19c 1275 break;
yihui 0:972f3778c19c 1276 default:
yihui 0:972f3778c19c 1277 return -1;
yihui 0:972f3778c19c 1278 }
yihui 0:972f3778c19c 1279 if (st.chip_cfg.accel_half)
yihui 0:972f3778c19c 1280 fsr[0] <<= 1;
yihui 0:972f3778c19c 1281 return 0;
yihui 0:972f3778c19c 1282 }
yihui 0:972f3778c19c 1283
yihui 0:972f3778c19c 1284 /**
yihui 0:972f3778c19c 1285 * @brief Set the accel full-scale range.
yihui 0:972f3778c19c 1286 * @param[in] fsr Desired full-scale range.
yihui 0:972f3778c19c 1287 * @return 0 if successful.
yihui 0:972f3778c19c 1288 */
yihui 0:972f3778c19c 1289 int mpu_set_accel_fsr(unsigned char fsr)
yihui 0:972f3778c19c 1290 {
yihui 0:972f3778c19c 1291 unsigned char data;
yihui 0:972f3778c19c 1292
yihui 0:972f3778c19c 1293 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1294 return -1;
yihui 0:972f3778c19c 1295
yihui 0:972f3778c19c 1296 switch (fsr) {
yihui 0:972f3778c19c 1297 case 2:
yihui 0:972f3778c19c 1298 data = INV_FSR_2G << 3;
yihui 0:972f3778c19c 1299 break;
yihui 0:972f3778c19c 1300 case 4:
yihui 0:972f3778c19c 1301 data = INV_FSR_4G << 3;
yihui 0:972f3778c19c 1302 break;
yihui 0:972f3778c19c 1303 case 8:
yihui 0:972f3778c19c 1304 data = INV_FSR_8G << 3;
yihui 0:972f3778c19c 1305 break;
yihui 0:972f3778c19c 1306 case 16:
yihui 0:972f3778c19c 1307 data = INV_FSR_16G << 3;
yihui 0:972f3778c19c 1308 break;
yihui 0:972f3778c19c 1309 default:
yihui 0:972f3778c19c 1310 return -1;
yihui 0:972f3778c19c 1311 }
yihui 0:972f3778c19c 1312
yihui 0:972f3778c19c 1313 if (st.chip_cfg.accel_fsr == (data >> 3))
yihui 0:972f3778c19c 1314 return 0;
yihui 0:972f3778c19c 1315 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
yihui 0:972f3778c19c 1316 return -1;
yihui 0:972f3778c19c 1317 st.chip_cfg.accel_fsr = data >> 3;
yihui 0:972f3778c19c 1318 return 0;
yihui 0:972f3778c19c 1319 }
yihui 0:972f3778c19c 1320
yihui 0:972f3778c19c 1321 /**
yihui 0:972f3778c19c 1322 * @brief Get the current DLPF setting.
yihui 0:972f3778c19c 1323 * @param[out] lpf Current LPF setting.
yihui 0:972f3778c19c 1324 * 0 if successful.
yihui 0:972f3778c19c 1325 */
yihui 0:972f3778c19c 1326 int mpu_get_lpf(unsigned short *lpf)
yihui 0:972f3778c19c 1327 {
yihui 0:972f3778c19c 1328 switch (st.chip_cfg.lpf) {
yihui 0:972f3778c19c 1329 case INV_FILTER_188HZ:
yihui 0:972f3778c19c 1330 lpf[0] = 188;
yihui 0:972f3778c19c 1331 break;
yihui 0:972f3778c19c 1332 case INV_FILTER_98HZ:
yihui 0:972f3778c19c 1333 lpf[0] = 98;
yihui 0:972f3778c19c 1334 break;
yihui 0:972f3778c19c 1335 case INV_FILTER_42HZ:
yihui 0:972f3778c19c 1336 lpf[0] = 42;
yihui 0:972f3778c19c 1337 break;
yihui 0:972f3778c19c 1338 case INV_FILTER_20HZ:
yihui 0:972f3778c19c 1339 lpf[0] = 20;
yihui 0:972f3778c19c 1340 break;
yihui 0:972f3778c19c 1341 case INV_FILTER_10HZ:
yihui 0:972f3778c19c 1342 lpf[0] = 10;
yihui 0:972f3778c19c 1343 break;
yihui 0:972f3778c19c 1344 case INV_FILTER_5HZ:
yihui 0:972f3778c19c 1345 lpf[0] = 5;
yihui 0:972f3778c19c 1346 break;
yihui 0:972f3778c19c 1347 case INV_FILTER_256HZ_NOLPF2:
yihui 0:972f3778c19c 1348 case INV_FILTER_2100HZ_NOLPF:
yihui 0:972f3778c19c 1349 default:
yihui 0:972f3778c19c 1350 lpf[0] = 0;
yihui 0:972f3778c19c 1351 break;
yihui 0:972f3778c19c 1352 }
yihui 0:972f3778c19c 1353 return 0;
yihui 0:972f3778c19c 1354 }
yihui 0:972f3778c19c 1355
yihui 0:972f3778c19c 1356 /**
yihui 0:972f3778c19c 1357 * @brief Set digital low pass filter.
yihui 0:972f3778c19c 1358 * The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
yihui 0:972f3778c19c 1359 * @param[in] lpf Desired LPF setting.
yihui 0:972f3778c19c 1360 * @return 0 if successful.
yihui 0:972f3778c19c 1361 */
yihui 0:972f3778c19c 1362 int mpu_set_lpf(unsigned short lpf)
yihui 0:972f3778c19c 1363 {
yihui 0:972f3778c19c 1364 unsigned char data;
yihui 0:972f3778c19c 1365
yihui 0:972f3778c19c 1366 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1367 return -1;
yihui 0:972f3778c19c 1368
yihui 0:972f3778c19c 1369 if (lpf >= 188)
yihui 0:972f3778c19c 1370 data = INV_FILTER_188HZ;
yihui 0:972f3778c19c 1371 else if (lpf >= 98)
yihui 0:972f3778c19c 1372 data = INV_FILTER_98HZ;
yihui 0:972f3778c19c 1373 else if (lpf >= 42)
yihui 0:972f3778c19c 1374 data = INV_FILTER_42HZ;
yihui 0:972f3778c19c 1375 else if (lpf >= 20)
yihui 0:972f3778c19c 1376 data = INV_FILTER_20HZ;
yihui 0:972f3778c19c 1377 else if (lpf >= 10)
yihui 0:972f3778c19c 1378 data = INV_FILTER_10HZ;
yihui 0:972f3778c19c 1379 else
yihui 0:972f3778c19c 1380 data = INV_FILTER_5HZ;
yihui 0:972f3778c19c 1381
yihui 0:972f3778c19c 1382 if (st.chip_cfg.lpf == data)
yihui 0:972f3778c19c 1383 return 0;
yihui 0:972f3778c19c 1384 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, &data))
yihui 0:972f3778c19c 1385 return -1;
yihui 0:972f3778c19c 1386 st.chip_cfg.lpf = data;
yihui 0:972f3778c19c 1387 return 0;
yihui 0:972f3778c19c 1388 }
yihui 0:972f3778c19c 1389
yihui 0:972f3778c19c 1390 /**
yihui 0:972f3778c19c 1391 * @brief Get sampling rate.
yihui 0:972f3778c19c 1392 * @param[out] rate Current sampling rate (Hz).
yihui 0:972f3778c19c 1393 * @return 0 if successful.
yihui 0:972f3778c19c 1394 */
yihui 0:972f3778c19c 1395 int mpu_get_sample_rate(unsigned short *rate)
yihui 0:972f3778c19c 1396 {
yihui 0:972f3778c19c 1397 if (st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1398 return -1;
yihui 0:972f3778c19c 1399 else
yihui 0:972f3778c19c 1400 rate[0] = st.chip_cfg.sample_rate;
yihui 0:972f3778c19c 1401 return 0;
yihui 0:972f3778c19c 1402 }
yihui 0:972f3778c19c 1403
yihui 0:972f3778c19c 1404 /**
yihui 0:972f3778c19c 1405 * @brief Set sampling rate.
yihui 0:972f3778c19c 1406 * Sampling rate must be between 4Hz and 1kHz.
yihui 0:972f3778c19c 1407 * @param[in] rate Desired sampling rate (Hz).
yihui 0:972f3778c19c 1408 * @return 0 if successful.
yihui 0:972f3778c19c 1409 */
yihui 0:972f3778c19c 1410 int mpu_set_sample_rate(unsigned short rate)
yihui 0:972f3778c19c 1411 {
yihui 0:972f3778c19c 1412 unsigned char data;
yihui 0:972f3778c19c 1413
yihui 0:972f3778c19c 1414 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1415 return -1;
yihui 0:972f3778c19c 1416
yihui 0:972f3778c19c 1417 if (st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1418 return -1;
yihui 0:972f3778c19c 1419 else {
yihui 0:972f3778c19c 1420 if (st.chip_cfg.lp_accel_mode) {
yihui 0:972f3778c19c 1421 if (rate && (rate <= 40)) {
yihui 0:972f3778c19c 1422 /* Just stay in low-power accel mode. */
yihui 0:972f3778c19c 1423 mpu_lp_accel_mode(rate);
yihui 0:972f3778c19c 1424 return 0;
yihui 0:972f3778c19c 1425 }
yihui 0:972f3778c19c 1426 /* Requested rate exceeds the allowed frequencies in LP accel mode,
yihui 0:972f3778c19c 1427 * switch back to full-power mode.
yihui 0:972f3778c19c 1428 */
yihui 0:972f3778c19c 1429 mpu_lp_accel_mode(0);
yihui 0:972f3778c19c 1430 }
yihui 0:972f3778c19c 1431 if (rate < 4)
yihui 0:972f3778c19c 1432 rate = 4;
yihui 0:972f3778c19c 1433 else if (rate > 1000)
yihui 0:972f3778c19c 1434 rate = 1000;
yihui 0:972f3778c19c 1435
yihui 0:972f3778c19c 1436 data = 1000 / rate - 1;
yihui 0:972f3778c19c 1437 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, &data))
yihui 0:972f3778c19c 1438 return -1;
yihui 0:972f3778c19c 1439
yihui 0:972f3778c19c 1440 st.chip_cfg.sample_rate = 1000 / (1 + data);
yihui 0:972f3778c19c 1441
yihui 0:972f3778c19c 1442 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 1443 mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
yihui 0:972f3778c19c 1444 #endif
yihui 0:972f3778c19c 1445
yihui 0:972f3778c19c 1446 /* Automatically set LPF to 1/2 sampling rate. */
yihui 0:972f3778c19c 1447 mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
yihui 0:972f3778c19c 1448 return 0;
yihui 0:972f3778c19c 1449 }
yihui 0:972f3778c19c 1450 }
yihui 0:972f3778c19c 1451
yihui 0:972f3778c19c 1452 /**
yihui 0:972f3778c19c 1453 * @brief Get compass sampling rate.
yihui 0:972f3778c19c 1454 * @param[out] rate Current compass sampling rate (Hz).
yihui 0:972f3778c19c 1455 * @return 0 if successful.
yihui 0:972f3778c19c 1456 */
yihui 0:972f3778c19c 1457 int mpu_get_compass_sample_rate(unsigned short *rate)
yihui 0:972f3778c19c 1458 {
yihui 0:972f3778c19c 1459 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 1460 rate[0] = st.chip_cfg.compass_sample_rate;
yihui 0:972f3778c19c 1461 return 0;
yihui 0:972f3778c19c 1462 #else
yihui 0:972f3778c19c 1463 rate[0] = 0;
yihui 0:972f3778c19c 1464 return -1;
yihui 0:972f3778c19c 1465 #endif
yihui 0:972f3778c19c 1466 }
yihui 0:972f3778c19c 1467
yihui 0:972f3778c19c 1468 /**
yihui 0:972f3778c19c 1469 * @brief Set compass sampling rate.
yihui 0:972f3778c19c 1470 * The compass on the auxiliary I2C bus is read by the MPU hardware at a
yihui 0:972f3778c19c 1471 * maximum of 100Hz. The actual rate can be set to a fraction of the gyro
yihui 0:972f3778c19c 1472 * sampling rate.
yihui 0:972f3778c19c 1473 *
yihui 0:972f3778c19c 1474 * \n WARNING: The new rate may be different than what was requested. Call
yihui 0:972f3778c19c 1475 * mpu_get_compass_sample_rate to check the actual setting.
yihui 0:972f3778c19c 1476 * @param[in] rate Desired compass sampling rate (Hz).
yihui 0:972f3778c19c 1477 * @return 0 if successful.
yihui 0:972f3778c19c 1478 */
yihui 0:972f3778c19c 1479 int mpu_set_compass_sample_rate(unsigned short rate)
yihui 0:972f3778c19c 1480 {
yihui 0:972f3778c19c 1481 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 1482 unsigned char div;
yihui 0:972f3778c19c 1483 if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
yihui 0:972f3778c19c 1484 return -1;
yihui 0:972f3778c19c 1485
yihui 0:972f3778c19c 1486 div = st.chip_cfg.sample_rate / rate - 1;
yihui 0:972f3778c19c 1487 if (mpu_hal_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
yihui 0:972f3778c19c 1488 return -1;
yihui 0:972f3778c19c 1489 st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
yihui 0:972f3778c19c 1490 return 0;
yihui 0:972f3778c19c 1491 #else
yihui 0:972f3778c19c 1492 return -1;
yihui 0:972f3778c19c 1493 #endif
yihui 0:972f3778c19c 1494 }
yihui 0:972f3778c19c 1495
yihui 0:972f3778c19c 1496 /**
yihui 0:972f3778c19c 1497 * @brief Get gyro sensitivity scale factor.
yihui 0:972f3778c19c 1498 * @param[out] sens Conversion from hardware units to dps.
yihui 0:972f3778c19c 1499 * @return 0 if successful.
yihui 0:972f3778c19c 1500 */
yihui 0:972f3778c19c 1501 int mpu_get_gyro_sens(float *sens)
yihui 0:972f3778c19c 1502 {
yihui 0:972f3778c19c 1503 switch (st.chip_cfg.gyro_fsr) {
yihui 0:972f3778c19c 1504 case INV_FSR_250DPS:
yihui 0:972f3778c19c 1505 sens[0] = 131.f;
yihui 0:972f3778c19c 1506 break;
yihui 0:972f3778c19c 1507 case INV_FSR_500DPS:
yihui 0:972f3778c19c 1508 sens[0] = 65.5f;
yihui 0:972f3778c19c 1509 break;
yihui 0:972f3778c19c 1510 case INV_FSR_1000DPS:
yihui 0:972f3778c19c 1511 sens[0] = 32.8f;
yihui 0:972f3778c19c 1512 break;
yihui 0:972f3778c19c 1513 case INV_FSR_2000DPS:
yihui 0:972f3778c19c 1514 sens[0] = 16.4f;
yihui 0:972f3778c19c 1515 break;
yihui 0:972f3778c19c 1516 default:
yihui 0:972f3778c19c 1517 return -1;
yihui 0:972f3778c19c 1518 }
yihui 0:972f3778c19c 1519 return 0;
yihui 0:972f3778c19c 1520 }
yihui 0:972f3778c19c 1521
yihui 0:972f3778c19c 1522 /**
yihui 0:972f3778c19c 1523 * @brief Get accel sensitivity scale factor.
yihui 0:972f3778c19c 1524 * @param[out] sens Conversion from hardware units to g's.
yihui 0:972f3778c19c 1525 * @return 0 if successful.
yihui 0:972f3778c19c 1526 */
yihui 0:972f3778c19c 1527 int mpu_get_accel_sens(unsigned short *sens)
yihui 0:972f3778c19c 1528 {
yihui 0:972f3778c19c 1529 switch (st.chip_cfg.accel_fsr) {
yihui 0:972f3778c19c 1530 case INV_FSR_2G:
yihui 0:972f3778c19c 1531 sens[0] = 16384;
yihui 0:972f3778c19c 1532 break;
yihui 0:972f3778c19c 1533 case INV_FSR_4G:
yihui 0:972f3778c19c 1534 sens[0] = 8092;
yihui 0:972f3778c19c 1535 break;
yihui 0:972f3778c19c 1536 case INV_FSR_8G:
yihui 0:972f3778c19c 1537 sens[0] = 4096;
yihui 0:972f3778c19c 1538 break;
yihui 0:972f3778c19c 1539 case INV_FSR_16G:
yihui 0:972f3778c19c 1540 sens[0] = 2048;
yihui 0:972f3778c19c 1541 break;
yihui 0:972f3778c19c 1542 default:
yihui 0:972f3778c19c 1543 return -1;
yihui 0:972f3778c19c 1544 }
yihui 0:972f3778c19c 1545 if (st.chip_cfg.accel_half)
yihui 0:972f3778c19c 1546 sens[0] >>= 1;
yihui 0:972f3778c19c 1547 return 0;
yihui 0:972f3778c19c 1548 }
yihui 0:972f3778c19c 1549
yihui 0:972f3778c19c 1550 /**
yihui 0:972f3778c19c 1551 * @brief Get current FIFO configuration.
yihui 0:972f3778c19c 1552 * @e sensors can contain a combination of the following flags:
yihui 0:972f3778c19c 1553 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
yihui 0:972f3778c19c 1554 * \n INV_XYZ_GYRO
yihui 0:972f3778c19c 1555 * \n INV_XYZ_ACCEL
yihui 0:972f3778c19c 1556 * @param[out] sensors Mask of sensors in FIFO.
yihui 0:972f3778c19c 1557 * @return 0 if successful.
yihui 0:972f3778c19c 1558 */
yihui 0:972f3778c19c 1559 int mpu_get_fifo_config(unsigned char *sensors)
yihui 0:972f3778c19c 1560 {
yihui 0:972f3778c19c 1561 sensors[0] = st.chip_cfg.fifo_enable;
yihui 0:972f3778c19c 1562 return 0;
yihui 0:972f3778c19c 1563 }
yihui 0:972f3778c19c 1564
yihui 0:972f3778c19c 1565 /**
yihui 0:972f3778c19c 1566 * @brief Select which sensors are pushed to FIFO.
yihui 0:972f3778c19c 1567 * @e sensors can contain a combination of the following flags:
yihui 0:972f3778c19c 1568 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
yihui 0:972f3778c19c 1569 * \n INV_XYZ_GYRO
yihui 0:972f3778c19c 1570 * \n INV_XYZ_ACCEL
yihui 0:972f3778c19c 1571 * @param[in] sensors Mask of sensors to push to FIFO.
yihui 0:972f3778c19c 1572 * @return 0 if successful.
yihui 0:972f3778c19c 1573 */
yihui 0:972f3778c19c 1574 int mpu_configure_fifo(unsigned char sensors)
yihui 0:972f3778c19c 1575 {
yihui 0:972f3778c19c 1576 unsigned char prev;
yihui 0:972f3778c19c 1577 int result = 0;
yihui 0:972f3778c19c 1578
yihui 0:972f3778c19c 1579 /* Compass data isn't going into the FIFO. Stop trying. */
yihui 0:972f3778c19c 1580 sensors &= ~INV_XYZ_COMPASS;
yihui 0:972f3778c19c 1581
yihui 0:972f3778c19c 1582 if (st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1583 return 0;
yihui 0:972f3778c19c 1584 else {
yihui 0:972f3778c19c 1585 if (!(st.chip_cfg.sensors))
yihui 0:972f3778c19c 1586 return -1;
yihui 0:972f3778c19c 1587 prev = st.chip_cfg.fifo_enable;
yihui 0:972f3778c19c 1588 st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
yihui 0:972f3778c19c 1589 if (st.chip_cfg.fifo_enable != sensors)
yihui 0:972f3778c19c 1590 /* You're not getting what you asked for. Some sensors are
yihui 0:972f3778c19c 1591 * asleep.
yihui 0:972f3778c19c 1592 */
yihui 0:972f3778c19c 1593 result = -1;
yihui 0:972f3778c19c 1594 else
yihui 0:972f3778c19c 1595 result = 0;
yihui 0:972f3778c19c 1596 if (sensors || st.chip_cfg.lp_accel_mode)
yihui 0:972f3778c19c 1597 set_int_enable(1);
yihui 0:972f3778c19c 1598 else
yihui 0:972f3778c19c 1599 set_int_enable(0);
yihui 0:972f3778c19c 1600 if (sensors) {
yihui 0:972f3778c19c 1601 if (mpu_reset_fifo()) {
yihui 0:972f3778c19c 1602 st.chip_cfg.fifo_enable = prev;
yihui 0:972f3778c19c 1603 return -1;
yihui 0:972f3778c19c 1604 }
yihui 0:972f3778c19c 1605 }
yihui 0:972f3778c19c 1606 }
yihui 0:972f3778c19c 1607
yihui 0:972f3778c19c 1608 return result;
yihui 0:972f3778c19c 1609 }
yihui 0:972f3778c19c 1610
yihui 0:972f3778c19c 1611 /**
yihui 0:972f3778c19c 1612 * @brief Get current power state.
yihui 0:972f3778c19c 1613 * @param[in] power_on 1 if turned on, 0 if suspended.
yihui 0:972f3778c19c 1614 * @return 0 if successful.
yihui 0:972f3778c19c 1615 */
yihui 0:972f3778c19c 1616 int mpu_get_power_state(unsigned char *power_on)
yihui 0:972f3778c19c 1617 {
yihui 0:972f3778c19c 1618 if (st.chip_cfg.sensors)
yihui 0:972f3778c19c 1619 power_on[0] = 1;
yihui 0:972f3778c19c 1620 else
yihui 0:972f3778c19c 1621 power_on[0] = 0;
yihui 0:972f3778c19c 1622 return 0;
yihui 0:972f3778c19c 1623 }
yihui 0:972f3778c19c 1624
yihui 0:972f3778c19c 1625 /**
yihui 0:972f3778c19c 1626 * @brief Turn specific sensors on/off.
yihui 0:972f3778c19c 1627 * @e sensors can contain a combination of the following flags:
yihui 0:972f3778c19c 1628 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
yihui 0:972f3778c19c 1629 * \n INV_XYZ_GYRO
yihui 0:972f3778c19c 1630 * \n INV_XYZ_ACCEL
yihui 0:972f3778c19c 1631 * \n INV_XYZ_COMPASS
yihui 0:972f3778c19c 1632 * @param[in] sensors Mask of sensors to wake.
yihui 0:972f3778c19c 1633 * @return 0 if successful.
yihui 0:972f3778c19c 1634 */
yihui 0:972f3778c19c 1635 int mpu_set_sensors(unsigned char sensors)
yihui 0:972f3778c19c 1636 {
yihui 0:972f3778c19c 1637 unsigned char data;
yihui 0:972f3778c19c 1638 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 1639 unsigned char user_ctrl;
yihui 0:972f3778c19c 1640 #endif
yihui 0:972f3778c19c 1641
yihui 0:972f3778c19c 1642 if (sensors & INV_XYZ_GYRO)
yihui 0:972f3778c19c 1643 data = INV_CLK_PLL;
yihui 0:972f3778c19c 1644 else if (sensors)
yihui 0:972f3778c19c 1645 data = 0;
yihui 0:972f3778c19c 1646 else
yihui 0:972f3778c19c 1647 data = BIT_SLEEP;
yihui 0:972f3778c19c 1648 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
yihui 0:972f3778c19c 1649 st.chip_cfg.sensors = 0;
yihui 0:972f3778c19c 1650 return -1;
yihui 0:972f3778c19c 1651 }
yihui 0:972f3778c19c 1652 st.chip_cfg.clk_src = data & ~BIT_SLEEP;
yihui 0:972f3778c19c 1653
yihui 0:972f3778c19c 1654 data = 0;
yihui 0:972f3778c19c 1655 if (!(sensors & INV_X_GYRO))
yihui 0:972f3778c19c 1656 data |= BIT_STBY_XG;
yihui 0:972f3778c19c 1657 if (!(sensors & INV_Y_GYRO))
yihui 0:972f3778c19c 1658 data |= BIT_STBY_YG;
yihui 0:972f3778c19c 1659 if (!(sensors & INV_Z_GYRO))
yihui 0:972f3778c19c 1660 data |= BIT_STBY_ZG;
yihui 0:972f3778c19c 1661 if (!(sensors & INV_XYZ_ACCEL))
yihui 0:972f3778c19c 1662 data |= BIT_STBY_XYZA;
yihui 0:972f3778c19c 1663 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
yihui 0:972f3778c19c 1664 st.chip_cfg.sensors = 0;
yihui 0:972f3778c19c 1665 return -1;
yihui 0:972f3778c19c 1666 }
yihui 0:972f3778c19c 1667
yihui 0:972f3778c19c 1668 if (sensors && (sensors != INV_XYZ_ACCEL))
yihui 0:972f3778c19c 1669 /* Latched interrupts only used in LP accel mode. */
yihui 0:972f3778c19c 1670 mpu_set_int_latched(0);
yihui 0:972f3778c19c 1671
yihui 0:972f3778c19c 1672 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 1673 #ifdef AK89xx_BYPASS
yihui 0:972f3778c19c 1674 if (sensors & INV_XYZ_COMPASS)
yihui 0:972f3778c19c 1675 mpu_set_bypass(1);
yihui 0:972f3778c19c 1676 else
yihui 0:972f3778c19c 1677 mpu_set_bypass(0);
yihui 0:972f3778c19c 1678 #else
yihui 0:972f3778c19c 1679 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
yihui 0:972f3778c19c 1680 return -1;
yihui 0:972f3778c19c 1681 /* Handle AKM power management. */
yihui 0:972f3778c19c 1682 if (sensors & INV_XYZ_COMPASS) {
yihui 0:972f3778c19c 1683 data = AKM_SINGLE_MEASUREMENT;
yihui 0:972f3778c19c 1684 user_ctrl |= BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1685 } else {
yihui 0:972f3778c19c 1686 data = AKM_POWER_DOWN;
yihui 0:972f3778c19c 1687 user_ctrl &= ~BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1688 }
yihui 0:972f3778c19c 1689 if (st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1690 user_ctrl |= BIT_DMP_EN;
yihui 0:972f3778c19c 1691 else
yihui 0:972f3778c19c 1692 user_ctrl &= ~BIT_DMP_EN;
yihui 0:972f3778c19c 1693 if (mpu_hal_write(st.hw->addr, st.reg->s1_do, 1, &data))
yihui 0:972f3778c19c 1694 return -1;
yihui 0:972f3778c19c 1695 /* Enable/disable I2C master mode. */
yihui 0:972f3778c19c 1696 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
yihui 0:972f3778c19c 1697 return -1;
yihui 0:972f3778c19c 1698 #endif
yihui 0:972f3778c19c 1699 #endif
yihui 0:972f3778c19c 1700
yihui 0:972f3778c19c 1701 st.chip_cfg.sensors = sensors;
yihui 0:972f3778c19c 1702 st.chip_cfg.lp_accel_mode = 0;
yihui 0:972f3778c19c 1703 delay_ms(50);
yihui 0:972f3778c19c 1704 return 0;
yihui 0:972f3778c19c 1705 }
yihui 0:972f3778c19c 1706
yihui 0:972f3778c19c 1707 /**
yihui 0:972f3778c19c 1708 * @brief Read the MPU interrupt status registers.
yihui 0:972f3778c19c 1709 * @param[out] status Mask of interrupt bits.
yihui 0:972f3778c19c 1710 * @return 0 if successful.
yihui 0:972f3778c19c 1711 */
yihui 0:972f3778c19c 1712 int mpu_get_int_status(short *status)
yihui 0:972f3778c19c 1713 {
yihui 0:972f3778c19c 1714 unsigned char tmp[2];
yihui 0:972f3778c19c 1715 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 1716 return -1;
yihui 0:972f3778c19c 1717 if (mpu_hal_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
yihui 0:972f3778c19c 1718 return -1;
yihui 0:972f3778c19c 1719 status[0] = (tmp[0] << 8) | tmp[1];
yihui 0:972f3778c19c 1720 return 0;
yihui 0:972f3778c19c 1721 }
yihui 0:972f3778c19c 1722
yihui 0:972f3778c19c 1723 /**
yihui 0:972f3778c19c 1724 * @brief Get one packet from the FIFO.
yihui 0:972f3778c19c 1725 * If @e sensors does not contain a particular sensor, disregard the data
yihui 0:972f3778c19c 1726 * returned to that pointer.
yihui 0:972f3778c19c 1727 * \n @e sensors can contain a combination of the following flags:
yihui 0:972f3778c19c 1728 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
yihui 0:972f3778c19c 1729 * \n INV_XYZ_GYRO
yihui 0:972f3778c19c 1730 * \n INV_XYZ_ACCEL
yihui 0:972f3778c19c 1731 * \n If the FIFO has no new data, @e sensors will be zero.
yihui 0:972f3778c19c 1732 * \n If the FIFO is disabled, @e sensors will be zero and this function will
yihui 0:972f3778c19c 1733 * return a non-zero error code.
yihui 0:972f3778c19c 1734 * @param[out] gyro Gyro data in hardware units.
yihui 0:972f3778c19c 1735 * @param[out] accel Accel data in hardware units.
yihui 0:972f3778c19c 1736 * @param[out] timestamp Timestamp in milliseconds.
yihui 0:972f3778c19c 1737 * @param[out] sensors Mask of sensors read from FIFO.
yihui 0:972f3778c19c 1738 * @param[out] more Number of remaining packets.
yihui 0:972f3778c19c 1739 * @return 0 if successful.
yihui 0:972f3778c19c 1740 */
yihui 0:972f3778c19c 1741 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
yihui 0:972f3778c19c 1742 unsigned char *sensors, unsigned char *more)
yihui 0:972f3778c19c 1743 {
yihui 0:972f3778c19c 1744 /* Assumes maximum packet size is gyro (6) + accel (6). */
yihui 0:972f3778c19c 1745 unsigned char data[MAX_PACKET_LENGTH];
yihui 0:972f3778c19c 1746 unsigned char packet_size = 0;
yihui 0:972f3778c19c 1747 unsigned short fifo_count, index = 0;
yihui 0:972f3778c19c 1748
yihui 0:972f3778c19c 1749 if (st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1750 return -1;
yihui 0:972f3778c19c 1751
yihui 0:972f3778c19c 1752 sensors[0] = 0;
yihui 0:972f3778c19c 1753 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 1754 return -1;
yihui 0:972f3778c19c 1755 if (!st.chip_cfg.fifo_enable)
yihui 0:972f3778c19c 1756 return -1;
yihui 0:972f3778c19c 1757
yihui 0:972f3778c19c 1758 if (st.chip_cfg.fifo_enable & INV_X_GYRO)
yihui 0:972f3778c19c 1759 packet_size += 2;
yihui 0:972f3778c19c 1760 if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
yihui 0:972f3778c19c 1761 packet_size += 2;
yihui 0:972f3778c19c 1762 if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
yihui 0:972f3778c19c 1763 packet_size += 2;
yihui 0:972f3778c19c 1764 if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
yihui 0:972f3778c19c 1765 packet_size += 6;
yihui 0:972f3778c19c 1766
yihui 0:972f3778c19c 1767 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
yihui 0:972f3778c19c 1768 return -1;
yihui 0:972f3778c19c 1769 fifo_count = (data[0] << 8) | data[1];
yihui 0:972f3778c19c 1770 if (fifo_count < packet_size)
yihui 0:972f3778c19c 1771 return 0;
yihui 0:972f3778c19c 1772 // log_i("FIFO count: %hd\n", fifo_count);
yihui 0:972f3778c19c 1773 if (fifo_count > (st.hw->max_fifo >> 1)) {
yihui 0:972f3778c19c 1774 /* FIFO is 50% full, better check overflow bit. */
yihui 0:972f3778c19c 1775 if (mpu_hal_read(st.hw->addr, st.reg->int_status, 1, data))
yihui 0:972f3778c19c 1776 return -1;
yihui 0:972f3778c19c 1777 if (data[0] & BIT_FIFO_OVERFLOW) {
yihui 0:972f3778c19c 1778 mpu_reset_fifo();
yihui 0:972f3778c19c 1779 return -2;
yihui 0:972f3778c19c 1780 }
yihui 0:972f3778c19c 1781 }
yihui 0:972f3778c19c 1782 get_ms((unsigned long*)timestamp);
yihui 0:972f3778c19c 1783
yihui 0:972f3778c19c 1784 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
yihui 0:972f3778c19c 1785 return -1;
yihui 0:972f3778c19c 1786 more[0] = fifo_count / packet_size - 1;
yihui 0:972f3778c19c 1787 sensors[0] = 0;
yihui 0:972f3778c19c 1788
yihui 0:972f3778c19c 1789 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
yihui 0:972f3778c19c 1790 accel[0] = (data[index+0] << 8) | data[index+1];
yihui 0:972f3778c19c 1791 accel[1] = (data[index+2] << 8) | data[index+3];
yihui 0:972f3778c19c 1792 accel[2] = (data[index+4] << 8) | data[index+5];
yihui 0:972f3778c19c 1793 sensors[0] |= INV_XYZ_ACCEL;
yihui 0:972f3778c19c 1794 index += 6;
yihui 0:972f3778c19c 1795 }
yihui 0:972f3778c19c 1796 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
yihui 0:972f3778c19c 1797 gyro[0] = (data[index+0] << 8) | data[index+1];
yihui 0:972f3778c19c 1798 sensors[0] |= INV_X_GYRO;
yihui 0:972f3778c19c 1799 index += 2;
yihui 0:972f3778c19c 1800 }
yihui 0:972f3778c19c 1801 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
yihui 0:972f3778c19c 1802 gyro[1] = (data[index+0] << 8) | data[index+1];
yihui 0:972f3778c19c 1803 sensors[0] |= INV_Y_GYRO;
yihui 0:972f3778c19c 1804 index += 2;
yihui 0:972f3778c19c 1805 }
yihui 0:972f3778c19c 1806 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
yihui 0:972f3778c19c 1807 gyro[2] = (data[index+0] << 8) | data[index+1];
yihui 0:972f3778c19c 1808 sensors[0] |= INV_Z_GYRO;
yihui 0:972f3778c19c 1809 index += 2;
yihui 0:972f3778c19c 1810 }
yihui 0:972f3778c19c 1811
yihui 0:972f3778c19c 1812 return 0;
yihui 0:972f3778c19c 1813 }
yihui 0:972f3778c19c 1814
yihui 0:972f3778c19c 1815 /**
yihui 0:972f3778c19c 1816 * @brief Get one unparsed packet from the FIFO.
yihui 0:972f3778c19c 1817 * This function should be used if the packet is to be parsed elsewhere.
yihui 0:972f3778c19c 1818 * @param[in] length Length of one FIFO packet.
yihui 0:972f3778c19c 1819 * @param[in] data FIFO packet.
yihui 0:972f3778c19c 1820 * @param[in] more Number of remaining packets.
yihui 0:972f3778c19c 1821 */
yihui 0:972f3778c19c 1822 int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
yihui 0:972f3778c19c 1823 unsigned char *more)
yihui 0:972f3778c19c 1824 {
yihui 0:972f3778c19c 1825 unsigned char tmp[2];
yihui 0:972f3778c19c 1826 unsigned short fifo_count;
yihui 0:972f3778c19c 1827 if (!st.chip_cfg.dmp_on)
yihui 0:972f3778c19c 1828 return -1;
yihui 0:972f3778c19c 1829 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 1830 return -1;
yihui 0:972f3778c19c 1831
yihui 0:972f3778c19c 1832 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
yihui 0:972f3778c19c 1833 return -1;
yihui 0:972f3778c19c 1834 fifo_count = (tmp[0] << 8) | tmp[1];
yihui 0:972f3778c19c 1835 if (fifo_count < length) {
yihui 0:972f3778c19c 1836 more[0] = 0;
yihui 0:972f3778c19c 1837 return -1;
yihui 0:972f3778c19c 1838 }
yihui 0:972f3778c19c 1839 if (fifo_count > (st.hw->max_fifo >> 1)) {
yihui 0:972f3778c19c 1840 /* FIFO is 50% full, better check overflow bit. */
yihui 0:972f3778c19c 1841 if (mpu_hal_read(st.hw->addr, st.reg->int_status, 1, tmp))
yihui 0:972f3778c19c 1842 return -1;
yihui 0:972f3778c19c 1843 if (tmp[0] & BIT_FIFO_OVERFLOW) {
yihui 0:972f3778c19c 1844 mpu_reset_fifo();
yihui 0:972f3778c19c 1845 return -2;
yihui 0:972f3778c19c 1846 }
yihui 0:972f3778c19c 1847 }
yihui 0:972f3778c19c 1848
yihui 0:972f3778c19c 1849 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, length, data))
yihui 0:972f3778c19c 1850 return -1;
yihui 0:972f3778c19c 1851 more[0] = fifo_count / length - 1;
yihui 0:972f3778c19c 1852 return 0;
yihui 0:972f3778c19c 1853 }
yihui 0:972f3778c19c 1854
yihui 0:972f3778c19c 1855 /**
yihui 0:972f3778c19c 1856 * @brief Set device to bypass mode.
yihui 0:972f3778c19c 1857 * @param[in] bypass_on 1 to enable bypass mode.
yihui 0:972f3778c19c 1858 * @return 0 if successful.
yihui 0:972f3778c19c 1859 */
yihui 0:972f3778c19c 1860 int mpu_set_bypass(unsigned char bypass_on)
yihui 0:972f3778c19c 1861 {
yihui 0:972f3778c19c 1862 unsigned char tmp;
yihui 0:972f3778c19c 1863
yihui 0:972f3778c19c 1864 if (st.chip_cfg.bypass_mode == bypass_on)
yihui 0:972f3778c19c 1865 return 0;
yihui 0:972f3778c19c 1866
yihui 0:972f3778c19c 1867 if (bypass_on) {
yihui 0:972f3778c19c 1868 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
yihui 0:972f3778c19c 1869 return -1;
yihui 0:972f3778c19c 1870 tmp &= ~BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1871 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
yihui 0:972f3778c19c 1872 return -1;
yihui 0:972f3778c19c 1873 delay_ms(3);
yihui 0:972f3778c19c 1874 tmp = BIT_BYPASS_EN;
yihui 0:972f3778c19c 1875 if (st.chip_cfg.active_low_int)
yihui 0:972f3778c19c 1876 tmp |= BIT_ACTL;
yihui 0:972f3778c19c 1877 if (st.chip_cfg.latched_int)
yihui 0:972f3778c19c 1878 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
yihui 0:972f3778c19c 1879 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
yihui 0:972f3778c19c 1880 return -1;
yihui 0:972f3778c19c 1881 } else {
yihui 0:972f3778c19c 1882 /* Enable I2C master mode if compass is being used. */
yihui 0:972f3778c19c 1883 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
yihui 0:972f3778c19c 1884 return -1;
yihui 0:972f3778c19c 1885 if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
yihui 0:972f3778c19c 1886 tmp |= BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1887 else
yihui 0:972f3778c19c 1888 tmp &= ~BIT_AUX_IF_EN;
yihui 0:972f3778c19c 1889 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
yihui 0:972f3778c19c 1890 return -1;
yihui 0:972f3778c19c 1891 delay_ms(3);
yihui 0:972f3778c19c 1892 if (st.chip_cfg.active_low_int)
yihui 0:972f3778c19c 1893 tmp = BIT_ACTL;
yihui 0:972f3778c19c 1894 else
yihui 0:972f3778c19c 1895 tmp = 0;
yihui 0:972f3778c19c 1896 if (st.chip_cfg.latched_int)
yihui 0:972f3778c19c 1897 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
yihui 0:972f3778c19c 1898 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
yihui 0:972f3778c19c 1899 return -1;
yihui 0:972f3778c19c 1900 }
yihui 0:972f3778c19c 1901 st.chip_cfg.bypass_mode = bypass_on;
yihui 0:972f3778c19c 1902 return 0;
yihui 0:972f3778c19c 1903 }
yihui 0:972f3778c19c 1904
yihui 0:972f3778c19c 1905 /**
yihui 0:972f3778c19c 1906 * @brief Set interrupt level.
yihui 0:972f3778c19c 1907 * @param[in] active_low 1 for active low, 0 for active high.
yihui 0:972f3778c19c 1908 * @return 0 if successful.
yihui 0:972f3778c19c 1909 */
yihui 0:972f3778c19c 1910 int mpu_set_int_level(unsigned char active_low)
yihui 0:972f3778c19c 1911 {
yihui 0:972f3778c19c 1912 st.chip_cfg.active_low_int = active_low;
yihui 0:972f3778c19c 1913 return 0;
yihui 0:972f3778c19c 1914 }
yihui 0:972f3778c19c 1915
yihui 0:972f3778c19c 1916 /**
yihui 0:972f3778c19c 1917 * @brief Enable latched interrupts.
yihui 0:972f3778c19c 1918 * Any MPU register will clear the interrupt.
yihui 0:972f3778c19c 1919 * @param[in] enable 1 to enable, 0 to disable.
yihui 0:972f3778c19c 1920 * @return 0 if successful.
yihui 0:972f3778c19c 1921 */
yihui 0:972f3778c19c 1922 int mpu_set_int_latched(unsigned char enable)
yihui 0:972f3778c19c 1923 {
yihui 0:972f3778c19c 1924 unsigned char tmp;
yihui 0:972f3778c19c 1925 if (st.chip_cfg.latched_int == enable)
yihui 0:972f3778c19c 1926 return 0;
yihui 0:972f3778c19c 1927
yihui 0:972f3778c19c 1928 if (enable)
yihui 0:972f3778c19c 1929 tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
yihui 0:972f3778c19c 1930 else
yihui 0:972f3778c19c 1931 tmp = 0;
yihui 0:972f3778c19c 1932 if (st.chip_cfg.bypass_mode)
yihui 0:972f3778c19c 1933 tmp |= BIT_BYPASS_EN;
yihui 0:972f3778c19c 1934 if (st.chip_cfg.active_low_int)
yihui 0:972f3778c19c 1935 tmp |= BIT_ACTL;
yihui 0:972f3778c19c 1936 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
yihui 0:972f3778c19c 1937 return -1;
yihui 0:972f3778c19c 1938 st.chip_cfg.latched_int = enable;
yihui 0:972f3778c19c 1939 return 0;
yihui 0:972f3778c19c 1940 }
yihui 0:972f3778c19c 1941
yihui 0:972f3778c19c 1942 #ifdef MPU6050
yihui 0:972f3778c19c 1943 static int get_accel_prod_shift(float *st_shift)
yihui 0:972f3778c19c 1944 {
yihui 0:972f3778c19c 1945 unsigned char tmp[4], shift_code[3], ii;
yihui 0:972f3778c19c 1946
yihui 0:972f3778c19c 1947 if (mpu_hal_read(st.hw->addr, 0x0D, 4, tmp))
yihui 0:972f3778c19c 1948 return 0x07;
yihui 0:972f3778c19c 1949
yihui 0:972f3778c19c 1950 shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
yihui 0:972f3778c19c 1951 shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
yihui 0:972f3778c19c 1952 shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
yihui 0:972f3778c19c 1953 for (ii = 0; ii < 3; ii++) {
yihui 0:972f3778c19c 1954 if (!shift_code[ii]) {
yihui 0:972f3778c19c 1955 st_shift[ii] = 0.f;
yihui 0:972f3778c19c 1956 continue;
yihui 0:972f3778c19c 1957 }
yihui 0:972f3778c19c 1958 /* Equivalent to..
yihui 0:972f3778c19c 1959 * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
yihui 0:972f3778c19c 1960 */
yihui 0:972f3778c19c 1961 st_shift[ii] = 0.34f;
yihui 0:972f3778c19c 1962 while (--shift_code[ii])
yihui 0:972f3778c19c 1963 st_shift[ii] *= 1.034f;
yihui 0:972f3778c19c 1964 }
yihui 0:972f3778c19c 1965 return 0;
yihui 0:972f3778c19c 1966 }
yihui 0:972f3778c19c 1967
yihui 0:972f3778c19c 1968 static int accel_self_test(long *bias_regular, long *bias_st)
yihui 0:972f3778c19c 1969 {
yihui 0:972f3778c19c 1970 int jj, result = 0;
yihui 0:972f3778c19c 1971 float st_shift[3], st_shift_cust, st_shift_var;
yihui 0:972f3778c19c 1972
yihui 0:972f3778c19c 1973 get_accel_prod_shift(st_shift);
yihui 0:972f3778c19c 1974 for(jj = 0; jj < 3; jj++) {
yihui 0:972f3778c19c 1975 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
yihui 0:972f3778c19c 1976 if (st_shift[jj]) {
yihui 0:972f3778c19c 1977 st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
yihui 0:972f3778c19c 1978 if (fabs(st_shift_var) > test.max_accel_var)
yihui 0:972f3778c19c 1979 result |= 1 << jj;
yihui 0:972f3778c19c 1980 } else if ((st_shift_cust < test.min_g) ||
yihui 0:972f3778c19c 1981 (st_shift_cust > test.max_g))
yihui 0:972f3778c19c 1982 result |= 1 << jj;
yihui 0:972f3778c19c 1983 }
yihui 0:972f3778c19c 1984
yihui 0:972f3778c19c 1985 return result;
yihui 0:972f3778c19c 1986 }
yihui 0:972f3778c19c 1987
yihui 0:972f3778c19c 1988 static int gyro_self_test(long *bias_regular, long *bias_st)
yihui 0:972f3778c19c 1989 {
yihui 0:972f3778c19c 1990 int jj, result = 0;
yihui 0:972f3778c19c 1991 unsigned char tmp[3];
yihui 0:972f3778c19c 1992 float st_shift, st_shift_cust, st_shift_var;
yihui 0:972f3778c19c 1993
yihui 0:972f3778c19c 1994 if (mpu_hal_read(st.hw->addr, 0x0D, 3, tmp))
yihui 0:972f3778c19c 1995 return 0x07;
yihui 0:972f3778c19c 1996
yihui 0:972f3778c19c 1997 tmp[0] &= 0x1F;
yihui 0:972f3778c19c 1998 tmp[1] &= 0x1F;
yihui 0:972f3778c19c 1999 tmp[2] &= 0x1F;
yihui 0:972f3778c19c 2000
yihui 0:972f3778c19c 2001 for (jj = 0; jj < 3; jj++) {
yihui 0:972f3778c19c 2002 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
yihui 0:972f3778c19c 2003 if (tmp[jj]) {
yihui 0:972f3778c19c 2004 st_shift = 3275.f / test.gyro_sens;
yihui 0:972f3778c19c 2005 while (--tmp[jj])
yihui 0:972f3778c19c 2006 st_shift *= 1.046f;
yihui 0:972f3778c19c 2007 st_shift_var = st_shift_cust / st_shift - 1.f;
yihui 0:972f3778c19c 2008 if (fabs(st_shift_var) > test.max_gyro_var)
yihui 0:972f3778c19c 2009 result |= 1 << jj;
yihui 0:972f3778c19c 2010 } else if ((st_shift_cust < test.min_dps) ||
yihui 0:972f3778c19c 2011 (st_shift_cust > test.max_dps))
yihui 0:972f3778c19c 2012 result |= 1 << jj;
yihui 0:972f3778c19c 2013 }
yihui 0:972f3778c19c 2014 return result;
yihui 0:972f3778c19c 2015 }
yihui 0:972f3778c19c 2016
yihui 0:972f3778c19c 2017 #endif
yihui 0:972f3778c19c 2018 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2019 static int compass_self_test(void)
yihui 0:972f3778c19c 2020 {
yihui 0:972f3778c19c 2021 int result = 0x07;
yihui 0:972f3778c19c 2022
yihui 0:972f3778c19c 2023 #ifndef MPU_USE_SPI
yihui 0:972f3778c19c 2024 unsigned char tmp[6];
yihui 0:972f3778c19c 2025 unsigned char tries = 10;
yihui 0:972f3778c19c 2026 short data;
yihui 0:972f3778c19c 2027
yihui 0:972f3778c19c 2028 mpu_set_bypass(1);
yihui 0:972f3778c19c 2029
yihui 0:972f3778c19c 2030 tmp[0] = AKM_POWER_DOWN;
yihui 0:972f3778c19c 2031 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
yihui 0:972f3778c19c 2032 return 0x07;
yihui 0:972f3778c19c 2033 tmp[0] = AKM_BIT_SELF_TEST;
yihui 0:972f3778c19c 2034 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
yihui 0:972f3778c19c 2035 goto AKM_restore;
yihui 0:972f3778c19c 2036 tmp[0] = AKM_MODE_SELF_TEST;
yihui 0:972f3778c19c 2037 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
yihui 0:972f3778c19c 2038 goto AKM_restore;
yihui 0:972f3778c19c 2039
yihui 0:972f3778c19c 2040 do {
yihui 0:972f3778c19c 2041 delay_ms(10);
yihui 0:972f3778c19c 2042 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
yihui 0:972f3778c19c 2043 goto AKM_restore;
yihui 0:972f3778c19c 2044 if (tmp[0] & AKM_DATA_READY)
yihui 0:972f3778c19c 2045 break;
yihui 0:972f3778c19c 2046 } while (tries--);
yihui 0:972f3778c19c 2047 if (!(tmp[0] & AKM_DATA_READY))
yihui 0:972f3778c19c 2048 goto AKM_restore;
yihui 0:972f3778c19c 2049
yihui 0:972f3778c19c 2050 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
yihui 0:972f3778c19c 2051 goto AKM_restore;
yihui 0:972f3778c19c 2052
yihui 0:972f3778c19c 2053 result = 0;
yihui 0:972f3778c19c 2054 #if defined MPU9150
yihui 0:972f3778c19c 2055 data = (short)(tmp[1] << 8) | tmp[0];
yihui 0:972f3778c19c 2056 if ((data > 100) || (data < -100))
yihui 0:972f3778c19c 2057 result |= 0x01;
yihui 0:972f3778c19c 2058 data = (short)(tmp[3] << 8) | tmp[2];
yihui 0:972f3778c19c 2059 if ((data > 100) || (data < -100))
yihui 0:972f3778c19c 2060 result |= 0x02;
yihui 0:972f3778c19c 2061 data = (short)(tmp[5] << 8) | tmp[4];
yihui 0:972f3778c19c 2062 if ((data > -300) || (data < -1000))
yihui 0:972f3778c19c 2063 result |= 0x04;
yihui 0:972f3778c19c 2064 #elif defined MPU9250
yihui 0:972f3778c19c 2065 data = (short)(tmp[1] << 8) | tmp[0];
yihui 0:972f3778c19c 2066 if ((data > 200) || (data < -200))
yihui 0:972f3778c19c 2067 result |= 0x01;
yihui 0:972f3778c19c 2068 data = (short)(tmp[3] << 8) | tmp[2];
yihui 0:972f3778c19c 2069 if ((data > 200) || (data < -200))
yihui 0:972f3778c19c 2070 result |= 0x02;
yihui 0:972f3778c19c 2071 data = (short)(tmp[5] << 8) | tmp[4];
yihui 0:972f3778c19c 2072 if ((data > -800) || (data < -3200))
yihui 0:972f3778c19c 2073 result |= 0x04;
yihui 0:972f3778c19c 2074 #endif
yihui 0:972f3778c19c 2075 AKM_restore:
yihui 0:972f3778c19c 2076 tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
yihui 0:972f3778c19c 2077 mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
yihui 0:972f3778c19c 2078 tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
yihui 0:972f3778c19c 2079 mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
yihui 0:972f3778c19c 2080 mpu_set_bypass(0);
yihui 0:972f3778c19c 2081 #else
yihui 0:972f3778c19c 2082 // to do - compass self test through spi interface
yihui 0:972f3778c19c 2083 #endif
yihui 0:972f3778c19c 2084
yihui 0:972f3778c19c 2085 return result;
yihui 0:972f3778c19c 2086 }
yihui 0:972f3778c19c 2087 #endif
yihui 0:972f3778c19c 2088
yihui 0:972f3778c19c 2089 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
yihui 0:972f3778c19c 2090 {
yihui 0:972f3778c19c 2091 unsigned char data[MAX_PACKET_LENGTH];
yihui 0:972f3778c19c 2092 unsigned char packet_count, ii;
yihui 0:972f3778c19c 2093 unsigned short fifo_count;
yihui 0:972f3778c19c 2094
yihui 0:972f3778c19c 2095 data[0] = 0x01;
yihui 0:972f3778c19c 2096 data[1] = 0;
yihui 0:972f3778c19c 2097 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
yihui 0:972f3778c19c 2098 return -1;
yihui 0:972f3778c19c 2099 delay_ms(200);
yihui 0:972f3778c19c 2100 data[0] = 0;
yihui 0:972f3778c19c 2101 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data))
yihui 0:972f3778c19c 2102 return -1;
yihui 0:972f3778c19c 2103 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2104 return -1;
yihui 0:972f3778c19c 2105 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
yihui 0:972f3778c19c 2106 return -1;
yihui 0:972f3778c19c 2107 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data))
yihui 0:972f3778c19c 2108 return -1;
yihui 0:972f3778c19c 2109 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2110 return -1;
yihui 0:972f3778c19c 2111 data[0] = BIT_FIFO_RST | BIT_DMP_RST;
yihui 0:972f3778c19c 2112 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2113 return -1;
yihui 0:972f3778c19c 2114 delay_ms(15);
yihui 0:972f3778c19c 2115 data[0] = st.test->reg_lpf;
yihui 0:972f3778c19c 2116 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, data))
yihui 0:972f3778c19c 2117 return -1;
yihui 0:972f3778c19c 2118 data[0] = st.test->reg_rate_div;
yihui 0:972f3778c19c 2119 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, data))
yihui 0:972f3778c19c 2120 return -1;
yihui 0:972f3778c19c 2121 if (hw_test)
yihui 0:972f3778c19c 2122 data[0] = st.test->reg_gyro_fsr | 0xE0;
yihui 0:972f3778c19c 2123 else
yihui 0:972f3778c19c 2124 data[0] = st.test->reg_gyro_fsr;
yihui 0:972f3778c19c 2125 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
yihui 0:972f3778c19c 2126 return -1;
yihui 0:972f3778c19c 2127
yihui 0:972f3778c19c 2128 if (hw_test)
yihui 0:972f3778c19c 2129 data[0] = st.test->reg_accel_fsr | 0xE0;
yihui 0:972f3778c19c 2130 else
yihui 0:972f3778c19c 2131 data[0] = test.reg_accel_fsr;
yihui 0:972f3778c19c 2132 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, data))
yihui 0:972f3778c19c 2133 return -1;
yihui 0:972f3778c19c 2134 if (hw_test)
yihui 0:972f3778c19c 2135 delay_ms(200);
yihui 0:972f3778c19c 2136
yihui 0:972f3778c19c 2137 /* Fill FIFO for test.wait_ms milliseconds. */
yihui 0:972f3778c19c 2138 data[0] = BIT_FIFO_EN;
yihui 0:972f3778c19c 2139 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2140 return -1;
yihui 0:972f3778c19c 2141
yihui 0:972f3778c19c 2142 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
yihui 0:972f3778c19c 2143 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2144 return -1;
yihui 0:972f3778c19c 2145 delay_ms(test.wait_ms);
yihui 0:972f3778c19c 2146 data[0] = 0;
yihui 0:972f3778c19c 2147 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2148 return -1;
yihui 0:972f3778c19c 2149
yihui 0:972f3778c19c 2150 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
yihui 0:972f3778c19c 2151 return -1;
yihui 0:972f3778c19c 2152
yihui 0:972f3778c19c 2153 fifo_count = (data[0] << 8) | data[1];
yihui 0:972f3778c19c 2154 packet_count = fifo_count / MAX_PACKET_LENGTH;
yihui 0:972f3778c19c 2155 gyro[0] = gyro[1] = gyro[2] = 0;
yihui 0:972f3778c19c 2156 accel[0] = accel[1] = accel[2] = 0;
yihui 0:972f3778c19c 2157
yihui 0:972f3778c19c 2158 for (ii = 0; ii < packet_count; ii++) {
yihui 0:972f3778c19c 2159 short accel_cur[3], gyro_cur[3];
yihui 0:972f3778c19c 2160 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
yihui 0:972f3778c19c 2161 return -1;
yihui 0:972f3778c19c 2162 accel_cur[0] = ((short)data[0] << 8) | data[1];
yihui 0:972f3778c19c 2163 accel_cur[1] = ((short)data[2] << 8) | data[3];
yihui 0:972f3778c19c 2164 accel_cur[2] = ((short)data[4] << 8) | data[5];
yihui 0:972f3778c19c 2165 accel[0] += (long)accel_cur[0];
yihui 0:972f3778c19c 2166 accel[1] += (long)accel_cur[1];
yihui 0:972f3778c19c 2167 accel[2] += (long)accel_cur[2];
yihui 0:972f3778c19c 2168 gyro_cur[0] = (((short)data[6] << 8) | data[7]);
yihui 0:972f3778c19c 2169 gyro_cur[1] = (((short)data[8] << 8) | data[9]);
yihui 0:972f3778c19c 2170 gyro_cur[2] = (((short)data[10] << 8) | data[11]);
yihui 0:972f3778c19c 2171 gyro[0] += (long)gyro_cur[0];
yihui 0:972f3778c19c 2172 gyro[1] += (long)gyro_cur[1];
yihui 0:972f3778c19c 2173 gyro[2] += (long)gyro_cur[2];
yihui 0:972f3778c19c 2174 }
yihui 0:972f3778c19c 2175 #ifdef EMPL_NO_64BIT
yihui 0:972f3778c19c 2176 gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2177 gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2178 gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2179 if (has_accel) {
yihui 0:972f3778c19c 2180 accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
yihui 0:972f3778c19c 2181 packet_count);
yihui 0:972f3778c19c 2182 accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
yihui 0:972f3778c19c 2183 packet_count);
yihui 0:972f3778c19c 2184 accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
yihui 0:972f3778c19c 2185 packet_count);
yihui 0:972f3778c19c 2186 /* Don't remove gravity! */
yihui 0:972f3778c19c 2187 accel[2] -= 65536L;
yihui 0:972f3778c19c 2188 }
yihui 0:972f3778c19c 2189 #else
yihui 0:972f3778c19c 2190 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2191 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2192 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
yihui 0:972f3778c19c 2193 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
yihui 0:972f3778c19c 2194 packet_count);
yihui 0:972f3778c19c 2195 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
yihui 0:972f3778c19c 2196 packet_count);
yihui 0:972f3778c19c 2197 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
yihui 0:972f3778c19c 2198 packet_count);
yihui 0:972f3778c19c 2199 /* Don't remove gravity! */
yihui 0:972f3778c19c 2200 if (accel[2] > 0L)
yihui 0:972f3778c19c 2201 accel[2] -= 65536L;
yihui 0:972f3778c19c 2202 else
yihui 0:972f3778c19c 2203 accel[2] += 65536L;
yihui 0:972f3778c19c 2204 #endif
yihui 0:972f3778c19c 2205
yihui 0:972f3778c19c 2206 return 0;
yihui 0:972f3778c19c 2207 }
yihui 0:972f3778c19c 2208
yihui 0:972f3778c19c 2209 #ifdef MPU6500
yihui 0:972f3778c19c 2210 #define REG_6500_XG_ST_DATA 0x0
yihui 0:972f3778c19c 2211 #define REG_6500_XA_ST_DATA 0xD
yihui 0:972f3778c19c 2212 static const unsigned short mpu_6500_st_tb[256] = {
yihui 0:972f3778c19c 2213 2620,2646,2672,2699,2726,2753,2781,2808, //7
yihui 0:972f3778c19c 2214 2837,2865,2894,2923,2952,2981,3011,3041, //15
yihui 0:972f3778c19c 2215 3072,3102,3133,3165,3196,3228,3261,3293, //23
yihui 0:972f3778c19c 2216 3326,3359,3393,3427,3461,3496,3531,3566, //31
yihui 0:972f3778c19c 2217 3602,3638,3674,3711,3748,3786,3823,3862, //39
yihui 0:972f3778c19c 2218 3900,3939,3979,4019,4059,4099,4140,4182, //47
yihui 0:972f3778c19c 2219 4224,4266,4308,4352,4395,4439,4483,4528, //55
yihui 0:972f3778c19c 2220 4574,4619,4665,4712,4759,4807,4855,4903, //63
yihui 0:972f3778c19c 2221 4953,5002,5052,5103,5154,5205,5257,5310, //71
yihui 0:972f3778c19c 2222 5363,5417,5471,5525,5581,5636,5693,5750, //79
yihui 0:972f3778c19c 2223 5807,5865,5924,5983,6043,6104,6165,6226, //87
yihui 0:972f3778c19c 2224 6289,6351,6415,6479,6544,6609,6675,6742, //95
yihui 0:972f3778c19c 2225 6810,6878,6946,7016,7086,7157,7229,7301, //103
yihui 0:972f3778c19c 2226 7374,7448,7522,7597,7673,7750,7828,7906, //111
yihui 0:972f3778c19c 2227 7985,8065,8145,8227,8309,8392,8476,8561, //119
yihui 0:972f3778c19c 2228 8647,8733,8820,8909,8998,9088,9178,9270,
yihui 0:972f3778c19c 2229 9363,9457,9551,9647,9743,9841,9939,10038,
yihui 0:972f3778c19c 2230 10139,10240,10343,10446,10550,10656,10763,10870,
yihui 0:972f3778c19c 2231 10979,11089,11200,11312,11425,11539,11654,11771,
yihui 0:972f3778c19c 2232 11889,12008,12128,12249,12371,12495,12620,12746,
yihui 0:972f3778c19c 2233 12874,13002,13132,13264,13396,13530,13666,13802,
yihui 0:972f3778c19c 2234 13940,14080,14221,14363,14506,14652,14798,14946,
yihui 0:972f3778c19c 2235 15096,15247,15399,15553,15709,15866,16024,16184,
yihui 0:972f3778c19c 2236 16346,16510,16675,16842,17010,17180,17352,17526,
yihui 0:972f3778c19c 2237 17701,17878,18057,18237,18420,18604,18790,18978,
yihui 0:972f3778c19c 2238 19167,19359,19553,19748,19946,20145,20347,20550,
yihui 0:972f3778c19c 2239 20756,20963,21173,21385,21598,21814,22033,22253,
yihui 0:972f3778c19c 2240 22475,22700,22927,23156,23388,23622,23858,24097,
yihui 0:972f3778c19c 2241 24338,24581,24827,25075,25326,25579,25835,26093,
yihui 0:972f3778c19c 2242 26354,26618,26884,27153,27424,27699,27976,28255,
yihui 0:972f3778c19c 2243 28538,28823,29112,29403,29697,29994,30294,30597,
yihui 0:972f3778c19c 2244 30903,31212,31524,31839,32157,32479,32804,33132
yihui 0:972f3778c19c 2245 };
yihui 0:972f3778c19c 2246 static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
yihui 0:972f3778c19c 2247 {
yihui 0:972f3778c19c 2248 int i, result = 0, otp_value_zero = 0;
yihui 0:972f3778c19c 2249 float accel_st_al_min, accel_st_al_max;
yihui 0:972f3778c19c 2250 float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max;
yihui 0:972f3778c19c 2251 unsigned char regs[3];
yihui 0:972f3778c19c 2252 if (mpu_hal_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) {
yihui 0:972f3778c19c 2253 if(debug)
yihui 0:972f3778c19c 2254 log_i("Reading OTP Register Error.\n");
yihui 0:972f3778c19c 2255 return 0x07;
yihui 0:972f3778c19c 2256 }
yihui 0:972f3778c19c 2257 if(debug)
yihui 0:972f3778c19c 2258 log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]);
yihui 0:972f3778c19c 2259 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2260 if (regs[i] != 0) {
yihui 0:972f3778c19c 2261 ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
yihui 0:972f3778c19c 2262 ct_shift_prod[i] *= 65536.f;
yihui 0:972f3778c19c 2263 ct_shift_prod[i] /= test.accel_sens;
yihui 0:972f3778c19c 2264 }
yihui 0:972f3778c19c 2265 else {
yihui 0:972f3778c19c 2266 ct_shift_prod[i] = 0;
yihui 0:972f3778c19c 2267 otp_value_zero = 1;
yihui 0:972f3778c19c 2268 }
yihui 0:972f3778c19c 2269 }
yihui 0:972f3778c19c 2270 if(otp_value_zero == 0) {
yihui 0:972f3778c19c 2271 if(debug)
yihui 0:972f3778c19c 2272 log_i("ACCEL:CRITERIA A\n");
yihui 0:972f3778c19c 2273 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2274 st_shift_cust[i] = bias_st[i] - bias_regular[i];
yihui 0:972f3778c19c 2275 if(debug) {
yihui 0:972f3778c19c 2276 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
yihui 0:972f3778c19c 2277 st_shift_cust[i]/1.f, bias_regular[i]/1.f,
yihui 0:972f3778c19c 2278 bias_st[i]/1.f);
yihui 0:972f3778c19c 2279 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
yihui 0:972f3778c19c 2280 }
yihui 0:972f3778c19c 2281
yihui 0:972f3778c19c 2282 st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
yihui 0:972f3778c19c 2283
yihui 0:972f3778c19c 2284 if(debug)
yihui 0:972f3778c19c 2285 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
yihui 0:972f3778c19c 2286 test.max_accel_var/1.f);
yihui 0:972f3778c19c 2287
yihui 0:972f3778c19c 2288 if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
yihui 0:972f3778c19c 2289 if(debug)
yihui 0:972f3778c19c 2290 log_i("ACCEL Fail Axis = %d\n", i);
yihui 0:972f3778c19c 2291 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2292 }
yihui 0:972f3778c19c 2293 }
yihui 0:972f3778c19c 2294 }
yihui 0:972f3778c19c 2295 else {
yihui 0:972f3778c19c 2296 /* Self Test Pass/Fail Criteria B */
yihui 0:972f3778c19c 2297 accel_st_al_min = test.min_g * 65536.f;
yihui 0:972f3778c19c 2298 accel_st_al_max = test.max_g * 65536.f;
yihui 0:972f3778c19c 2299
yihui 0:972f3778c19c 2300 if(debug) {
yihui 0:972f3778c19c 2301 log_i("ACCEL:CRITERIA B\r\n");
yihui 0:972f3778c19c 2302 log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f);
yihui 0:972f3778c19c 2303 log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f);
yihui 0:972f3778c19c 2304 }
yihui 0:972f3778c19c 2305
yihui 0:972f3778c19c 2306 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2307 st_shift_cust[i] = bias_st[i] - bias_regular[i];
yihui 0:972f3778c19c 2308
yihui 0:972f3778c19c 2309 if(debug)
yihui 0:972f3778c19c 2310 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);
yihui 0:972f3778c19c 2311 if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {
yihui 0:972f3778c19c 2312 if(debug)
yihui 0:972f3778c19c 2313 log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i);
yihui 0:972f3778c19c 2314 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2315 }
yihui 0:972f3778c19c 2316 }
yihui 0:972f3778c19c 2317 }
yihui 0:972f3778c19c 2318
yihui 0:972f3778c19c 2319 if(result == 0) {
yihui 0:972f3778c19c 2320 /* Self Test Pass/Fail Criteria C */
yihui 0:972f3778c19c 2321 accel_offset_max = test.max_g_offset * 65536.f;
yihui 0:972f3778c19c 2322 if(debug)
yihui 0:972f3778c19c 2323 log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f);
yihui 0:972f3778c19c 2324 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2325 if(fabs(bias_regular[i]) > accel_offset_max) {
yihui 0:972f3778c19c 2326 if(debug)
yihui 0:972f3778c19c 2327 log_i("FAILED: Accel axis:%d = %d > 500mg\n", i, bias_regular[i]);
yihui 0:972f3778c19c 2328 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2329 }
yihui 0:972f3778c19c 2330 }
yihui 0:972f3778c19c 2331 }
yihui 0:972f3778c19c 2332
yihui 0:972f3778c19c 2333 return result;
yihui 0:972f3778c19c 2334 }
yihui 0:972f3778c19c 2335
yihui 0:972f3778c19c 2336 static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
yihui 0:972f3778c19c 2337 {
yihui 0:972f3778c19c 2338 int i, result = 0, otp_value_zero = 0;
yihui 0:972f3778c19c 2339 float gyro_st_al_max;
yihui 0:972f3778c19c 2340 float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max;
yihui 0:972f3778c19c 2341 unsigned char regs[3];
yihui 0:972f3778c19c 2342
yihui 0:972f3778c19c 2343 if (mpu_hal_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) {
yihui 0:972f3778c19c 2344 if(debug)
yihui 0:972f3778c19c 2345 log_i("Reading OTP Register Error.\n");
yihui 0:972f3778c19c 2346 return 0x07;
yihui 0:972f3778c19c 2347 }
yihui 0:972f3778c19c 2348
yihui 0:972f3778c19c 2349 if(debug)
yihui 0:972f3778c19c 2350 log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]);
yihui 0:972f3778c19c 2351
yihui 0:972f3778c19c 2352 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2353 if (regs[i] != 0) {
yihui 0:972f3778c19c 2354 ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
yihui 0:972f3778c19c 2355 ct_shift_prod[i] *= 65536.f;
yihui 0:972f3778c19c 2356 ct_shift_prod[i] /= test.gyro_sens;
yihui 0:972f3778c19c 2357 }
yihui 0:972f3778c19c 2358 else {
yihui 0:972f3778c19c 2359 ct_shift_prod[i] = 0;
yihui 0:972f3778c19c 2360 otp_value_zero = 1;
yihui 0:972f3778c19c 2361 }
yihui 0:972f3778c19c 2362 }
yihui 0:972f3778c19c 2363
yihui 0:972f3778c19c 2364 if(otp_value_zero == 0) {
yihui 0:972f3778c19c 2365 if(debug)
yihui 0:972f3778c19c 2366 log_i("GYRO:CRITERIA A\n");
yihui 0:972f3778c19c 2367 /* Self Test Pass/Fail Criteria A */
yihui 0:972f3778c19c 2368 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2369 st_shift_cust[i] = bias_st[i] - bias_regular[i];
yihui 0:972f3778c19c 2370
yihui 0:972f3778c19c 2371 if(debug) {
yihui 0:972f3778c19c 2372 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
yihui 0:972f3778c19c 2373 st_shift_cust[i]/1.f, bias_regular[i]/1.f,
yihui 0:972f3778c19c 2374 bias_st[i]/1.f);
yihui 0:972f3778c19c 2375 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
yihui 0:972f3778c19c 2376 }
yihui 0:972f3778c19c 2377
yihui 0:972f3778c19c 2378 st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
yihui 0:972f3778c19c 2379
yihui 0:972f3778c19c 2380 if(debug)
yihui 0:972f3778c19c 2381 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
yihui 0:972f3778c19c 2382 test.max_gyro_var/1.f);
yihui 0:972f3778c19c 2383
yihui 0:972f3778c19c 2384 if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
yihui 0:972f3778c19c 2385 if(debug)
yihui 0:972f3778c19c 2386 log_i("Gyro Fail Axis = %d\n", i);
yihui 0:972f3778c19c 2387 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2388 }
yihui 0:972f3778c19c 2389 }
yihui 0:972f3778c19c 2390 }
yihui 0:972f3778c19c 2391 else {
yihui 0:972f3778c19c 2392 /* Self Test Pass/Fail Criteria B */
yihui 0:972f3778c19c 2393 gyro_st_al_max = test.max_dps * 65536.f;
yihui 0:972f3778c19c 2394
yihui 0:972f3778c19c 2395 if(debug) {
yihui 0:972f3778c19c 2396 log_i("GYRO:CRITERIA B\r\n");
yihui 0:972f3778c19c 2397 log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
yihui 0:972f3778c19c 2398 }
yihui 0:972f3778c19c 2399
yihui 0:972f3778c19c 2400 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2401 st_shift_cust[i] = bias_st[i] - bias_regular[i];
yihui 0:972f3778c19c 2402
yihui 0:972f3778c19c 2403 if(debug)
yihui 0:972f3778c19c 2404 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);
yihui 0:972f3778c19c 2405 if(st_shift_cust[i] < gyro_st_al_max) {
yihui 0:972f3778c19c 2406 if(debug)
yihui 0:972f3778c19c 2407 log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
yihui 0:972f3778c19c 2408 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2409 }
yihui 0:972f3778c19c 2410 }
yihui 0:972f3778c19c 2411 }
yihui 0:972f3778c19c 2412
yihui 0:972f3778c19c 2413 if(result == 0) {
yihui 0:972f3778c19c 2414 /* Self Test Pass/Fail Criteria C */
yihui 0:972f3778c19c 2415 gyro_offset_max = test.min_dps * 65536.f;
yihui 0:972f3778c19c 2416 if(debug)
yihui 0:972f3778c19c 2417 log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f);
yihui 0:972f3778c19c 2418 for (i = 0; i < 3; i++) {
yihui 0:972f3778c19c 2419 if(fabs(bias_regular[i]) > gyro_offset_max) {
yihui 0:972f3778c19c 2420 if(debug)
yihui 0:972f3778c19c 2421 log_i("FAILED: Gyro axis:%d = %d > 20dps\n", i, bias_regular[i]);
yihui 0:972f3778c19c 2422 result |= 1 << i; //Error condition
yihui 0:972f3778c19c 2423 }
yihui 0:972f3778c19c 2424 }
yihui 0:972f3778c19c 2425 }
yihui 0:972f3778c19c 2426 return result;
yihui 0:972f3778c19c 2427 }
yihui 0:972f3778c19c 2428
yihui 0:972f3778c19c 2429 static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug)
yihui 0:972f3778c19c 2430 {
yihui 0:972f3778c19c 2431 unsigned char data[HWST_MAX_PACKET_LENGTH];
yihui 0:972f3778c19c 2432 unsigned char packet_count, ii;
yihui 0:972f3778c19c 2433 unsigned short fifo_count;
yihui 0:972f3778c19c 2434 int s = 0, read_size = 0, ind;
yihui 0:972f3778c19c 2435
yihui 0:972f3778c19c 2436 data[0] = 0x01;
yihui 0:972f3778c19c 2437 data[1] = 0;
yihui 0:972f3778c19c 2438 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
yihui 0:972f3778c19c 2439 return -1;
yihui 0:972f3778c19c 2440 delay_ms(200);
yihui 0:972f3778c19c 2441 data[0] = 0;
yihui 0:972f3778c19c 2442 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data))
yihui 0:972f3778c19c 2443 return -1;
yihui 0:972f3778c19c 2444 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2445 return -1;
yihui 0:972f3778c19c 2446 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
yihui 0:972f3778c19c 2447 return -1;
yihui 0:972f3778c19c 2448 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data))
yihui 0:972f3778c19c 2449 return -1;
yihui 0:972f3778c19c 2450 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2451 return -1;
yihui 0:972f3778c19c 2452 data[0] = BIT_FIFO_RST | BIT_DMP_RST;
yihui 0:972f3778c19c 2453 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2454 return -1;
yihui 0:972f3778c19c 2455 delay_ms(15);
yihui 0:972f3778c19c 2456 data[0] = st.test->reg_lpf;
yihui 0:972f3778c19c 2457 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, data))
yihui 0:972f3778c19c 2458 return -1;
yihui 0:972f3778c19c 2459 data[0] = st.test->reg_rate_div;
yihui 0:972f3778c19c 2460 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, data))
yihui 0:972f3778c19c 2461 return -1;
yihui 0:972f3778c19c 2462 if (hw_test)
yihui 0:972f3778c19c 2463 data[0] = st.test->reg_gyro_fsr | 0xE0;
yihui 0:972f3778c19c 2464 else
yihui 0:972f3778c19c 2465 data[0] = st.test->reg_gyro_fsr;
yihui 0:972f3778c19c 2466 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
yihui 0:972f3778c19c 2467 return -1;
yihui 0:972f3778c19c 2468
yihui 0:972f3778c19c 2469 if (hw_test)
yihui 0:972f3778c19c 2470 data[0] = st.test->reg_accel_fsr | 0xE0;
yihui 0:972f3778c19c 2471 else
yihui 0:972f3778c19c 2472 data[0] = test.reg_accel_fsr;
yihui 0:972f3778c19c 2473 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, data))
yihui 0:972f3778c19c 2474 return -1;
yihui 0:972f3778c19c 2475
yihui 0:972f3778c19c 2476 delay_ms(test.wait_ms); //wait 200ms for sensors to stabilize
yihui 0:972f3778c19c 2477
yihui 0:972f3778c19c 2478 /* Enable FIFO */
yihui 0:972f3778c19c 2479 data[0] = BIT_FIFO_EN;
yihui 0:972f3778c19c 2480 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data))
yihui 0:972f3778c19c 2481 return -1;
yihui 0:972f3778c19c 2482 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
yihui 0:972f3778c19c 2483 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2484 return -1;
yihui 0:972f3778c19c 2485
yihui 0:972f3778c19c 2486 //initialize the bias return values
yihui 0:972f3778c19c 2487 gyro[0] = gyro[1] = gyro[2] = 0;
yihui 0:972f3778c19c 2488 accel[0] = accel[1] = accel[2] = 0;
yihui 0:972f3778c19c 2489
yihui 0:972f3778c19c 2490 if(debug)
yihui 0:972f3778c19c 2491 log_i("Starting Bias Loop Reads\n");
yihui 0:972f3778c19c 2492
yihui 0:972f3778c19c 2493 //start reading samples
yihui 0:972f3778c19c 2494 while (s < test.packet_thresh) {
yihui 0:972f3778c19c 2495 delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO
yihui 0:972f3778c19c 2496 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
yihui 0:972f3778c19c 2497 return -1;
yihui 0:972f3778c19c 2498 fifo_count = (data[0] << 8) | data[1];
yihui 0:972f3778c19c 2499 packet_count = fifo_count / MAX_PACKET_LENGTH;
yihui 0:972f3778c19c 2500 if ((test.packet_thresh - s) < packet_count)
yihui 0:972f3778c19c 2501 packet_count = test.packet_thresh - s;
yihui 0:972f3778c19c 2502 read_size = packet_count * MAX_PACKET_LENGTH;
yihui 0:972f3778c19c 2503
yihui 0:972f3778c19c 2504 //burst read from FIFO
yihui 0:972f3778c19c 2505 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, read_size, data))
yihui 0:972f3778c19c 2506 return -1;
yihui 0:972f3778c19c 2507 ind = 0;
yihui 0:972f3778c19c 2508 for (ii = 0; ii < packet_count; ii++) {
yihui 0:972f3778c19c 2509 short accel_cur[3], gyro_cur[3];
yihui 0:972f3778c19c 2510 accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1];
yihui 0:972f3778c19c 2511 accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3];
yihui 0:972f3778c19c 2512 accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5];
yihui 0:972f3778c19c 2513 accel[0] += (long)accel_cur[0];
yihui 0:972f3778c19c 2514 accel[1] += (long)accel_cur[1];
yihui 0:972f3778c19c 2515 accel[2] += (long)accel_cur[2];
yihui 0:972f3778c19c 2516 gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]);
yihui 0:972f3778c19c 2517 gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]);
yihui 0:972f3778c19c 2518 gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]);
yihui 0:972f3778c19c 2519 gyro[0] += (long)gyro_cur[0];
yihui 0:972f3778c19c 2520 gyro[1] += (long)gyro_cur[1];
yihui 0:972f3778c19c 2521 gyro[2] += (long)gyro_cur[2];
yihui 0:972f3778c19c 2522 ind += MAX_PACKET_LENGTH;
yihui 0:972f3778c19c 2523 }
yihui 0:972f3778c19c 2524 s += packet_count;
yihui 0:972f3778c19c 2525 }
yihui 0:972f3778c19c 2526
yihui 0:972f3778c19c 2527 if(debug)
yihui 0:972f3778c19c 2528 log_i("Samples: %d\n", s);
yihui 0:972f3778c19c 2529
yihui 0:972f3778c19c 2530 //stop FIFO
yihui 0:972f3778c19c 2531 data[0] = 0;
yihui 0:972f3778c19c 2532 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data))
yihui 0:972f3778c19c 2533 return -1;
yihui 0:972f3778c19c 2534
yihui 0:972f3778c19c 2535 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s);
yihui 0:972f3778c19c 2536 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s);
yihui 0:972f3778c19c 2537 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s);
yihui 0:972f3778c19c 2538 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s);
yihui 0:972f3778c19c 2539 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s);
yihui 0:972f3778c19c 2540 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s);
yihui 0:972f3778c19c 2541 /* remove gravity from bias calculation */
yihui 0:972f3778c19c 2542 if (accel[2] > 0L)
yihui 0:972f3778c19c 2543 accel[2] -= 65536L;
yihui 0:972f3778c19c 2544 else
yihui 0:972f3778c19c 2545 accel[2] += 65536L;
yihui 0:972f3778c19c 2546
yihui 0:972f3778c19c 2547
yihui 0:972f3778c19c 2548 if(debug) {
yihui 0:972f3778c19c 2549 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);
yihui 0:972f3778c19c 2550 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);
yihui 0:972f3778c19c 2551 }
yihui 0:972f3778c19c 2552
yihui 0:972f3778c19c 2553 return 0;
yihui 0:972f3778c19c 2554 }
yihui 0:972f3778c19c 2555 /**
yihui 0:972f3778c19c 2556 * @brief Trigger gyro/accel/compass self-test for MPU6500/MPU9250
yihui 0:972f3778c19c 2557 * On success/error, the self-test returns a mask representing the sensor(s)
yihui 0:972f3778c19c 2558 * that failed. For each bit, a one (1) represents a "pass" case; conversely,
yihui 0:972f3778c19c 2559 * a zero (0) indicates a failure.
yihui 0:972f3778c19c 2560 *
yihui 0:972f3778c19c 2561 * \n The mask is defined as follows:
yihui 0:972f3778c19c 2562 * \n Bit 0: Gyro.
yihui 0:972f3778c19c 2563 * \n Bit 1: Accel.
yihui 0:972f3778c19c 2564 * \n Bit 2: Compass.
yihui 0:972f3778c19c 2565 *
yihui 0:972f3778c19c 2566 * @param[out] gyro Gyro biases in q16 format.
yihui 0:972f3778c19c 2567 * @param[out] accel Accel biases (if applicable) in q16 format.
yihui 0:972f3778c19c 2568 * @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver.
yihui 0:972f3778c19c 2569 * @return Result mask (see above).
yihui 0:972f3778c19c 2570 */
yihui 0:972f3778c19c 2571 int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
yihui 0:972f3778c19c 2572 {
yihui 0:972f3778c19c 2573 const unsigned char tries = 2;
yihui 0:972f3778c19c 2574 long gyro_st[3], accel_st[3];
yihui 0:972f3778c19c 2575 unsigned char accel_result, gyro_result;
yihui 0:972f3778c19c 2576 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2577 unsigned char compass_result;
yihui 0:972f3778c19c 2578 #endif
yihui 0:972f3778c19c 2579 int ii;
yihui 0:972f3778c19c 2580
yihui 0:972f3778c19c 2581 int result;
yihui 0:972f3778c19c 2582 unsigned char accel_fsr, fifo_sensors, sensors_on;
yihui 0:972f3778c19c 2583 unsigned short gyro_fsr, sample_rate, lpf;
yihui 0:972f3778c19c 2584 unsigned char dmp_was_on;
yihui 0:972f3778c19c 2585
yihui 0:972f3778c19c 2586
yihui 0:972f3778c19c 2587
yihui 0:972f3778c19c 2588 if(debug)
yihui 0:972f3778c19c 2589 log_i("Starting MPU6500 HWST!\r\n");
yihui 0:972f3778c19c 2590
yihui 0:972f3778c19c 2591 if (st.chip_cfg.dmp_on) {
yihui 0:972f3778c19c 2592 mpu_set_dmp_state(0);
yihui 0:972f3778c19c 2593 dmp_was_on = 1;
yihui 0:972f3778c19c 2594 } else
yihui 0:972f3778c19c 2595 dmp_was_on = 0;
yihui 0:972f3778c19c 2596
yihui 0:972f3778c19c 2597 /* Get initial settings. */
yihui 0:972f3778c19c 2598 mpu_get_gyro_fsr(&gyro_fsr);
yihui 0:972f3778c19c 2599 mpu_get_accel_fsr(&accel_fsr);
yihui 0:972f3778c19c 2600 mpu_get_lpf(&lpf);
yihui 0:972f3778c19c 2601 mpu_get_sample_rate(&sample_rate);
yihui 0:972f3778c19c 2602 sensors_on = st.chip_cfg.sensors;
yihui 0:972f3778c19c 2603 mpu_get_fifo_config(&fifo_sensors);
yihui 0:972f3778c19c 2604
yihui 0:972f3778c19c 2605 if(debug)
yihui 0:972f3778c19c 2606 log_i("Retrieving Biases\r\n");
yihui 0:972f3778c19c 2607
yihui 0:972f3778c19c 2608 for (ii = 0; ii < tries; ii++)
yihui 0:972f3778c19c 2609 if (!get_st_6500_biases(gyro, accel, 0, debug))
yihui 0:972f3778c19c 2610 break;
yihui 0:972f3778c19c 2611 if (ii == tries) {
yihui 0:972f3778c19c 2612 /* If we reach this point, we most likely encountered an I2C error.
yihui 0:972f3778c19c 2613 * We'll just report an error for all three sensors.
yihui 0:972f3778c19c 2614 */
yihui 0:972f3778c19c 2615 if(debug)
yihui 0:972f3778c19c 2616 log_i("Retrieving Biases Error - possible I2C error\n");
yihui 0:972f3778c19c 2617
yihui 0:972f3778c19c 2618 result = 0;
yihui 0:972f3778c19c 2619 goto restore;
yihui 0:972f3778c19c 2620 }
yihui 0:972f3778c19c 2621
yihui 0:972f3778c19c 2622 if(debug)
yihui 0:972f3778c19c 2623 log_i("Retrieving ST Biases\n");
yihui 0:972f3778c19c 2624
yihui 0:972f3778c19c 2625 for (ii = 0; ii < tries; ii++)
yihui 0:972f3778c19c 2626 if (!get_st_6500_biases(gyro_st, accel_st, 1, debug))
yihui 0:972f3778c19c 2627 break;
yihui 0:972f3778c19c 2628 if (ii == tries) {
yihui 0:972f3778c19c 2629
yihui 0:972f3778c19c 2630 if(debug)
yihui 0:972f3778c19c 2631 log_i("Retrieving ST Biases Error - possible I2C error\n");
yihui 0:972f3778c19c 2632
yihui 0:972f3778c19c 2633 /* Again, probably an I2C error. */
yihui 0:972f3778c19c 2634 result = 0;
yihui 0:972f3778c19c 2635 goto restore;
yihui 0:972f3778c19c 2636 }
yihui 0:972f3778c19c 2637
yihui 0:972f3778c19c 2638 accel_result = accel_6500_self_test(accel, accel_st, debug);
yihui 0:972f3778c19c 2639 if(debug)
yihui 0:972f3778c19c 2640 log_i("Accel Self Test Results: %d\n", accel_result);
yihui 0:972f3778c19c 2641
yihui 0:972f3778c19c 2642 gyro_result = gyro_6500_self_test(gyro, gyro_st, debug);
yihui 0:972f3778c19c 2643 if(debug)
yihui 0:972f3778c19c 2644 log_i("Gyro Self Test Results: %d\n", gyro_result);
yihui 0:972f3778c19c 2645
yihui 0:972f3778c19c 2646 result = 0;
yihui 0:972f3778c19c 2647 if (!gyro_result)
yihui 0:972f3778c19c 2648 result |= 0x01;
yihui 0:972f3778c19c 2649 if (!accel_result)
yihui 0:972f3778c19c 2650 result |= 0x02;
yihui 0:972f3778c19c 2651
yihui 0:972f3778c19c 2652 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2653 compass_result = compass_self_test();
yihui 0:972f3778c19c 2654 if(debug)
yihui 0:972f3778c19c 2655 log_i("Compass Self Test Results: %d\n", compass_result);
yihui 0:972f3778c19c 2656 if (!compass_result)
yihui 0:972f3778c19c 2657 result |= 0x04;
yihui 0:972f3778c19c 2658 #else
yihui 0:972f3778c19c 2659 result |= 0x04;
yihui 0:972f3778c19c 2660 #endif
yihui 0:972f3778c19c 2661 restore:
yihui 0:972f3778c19c 2662 if(debug)
yihui 0:972f3778c19c 2663 log_i("Exiting HWST\n");
yihui 0:972f3778c19c 2664 /* Set to invalid values to ensure no I2C writes are skipped. */
yihui 0:972f3778c19c 2665 st.chip_cfg.gyro_fsr = 0xFF;
yihui 0:972f3778c19c 2666 st.chip_cfg.accel_fsr = 0xFF;
yihui 0:972f3778c19c 2667 st.chip_cfg.lpf = 0xFF;
yihui 0:972f3778c19c 2668 st.chip_cfg.sample_rate = 0xFFFF;
yihui 0:972f3778c19c 2669 st.chip_cfg.sensors = 0xFF;
yihui 0:972f3778c19c 2670 st.chip_cfg.fifo_enable = 0xFF;
yihui 0:972f3778c19c 2671 st.chip_cfg.clk_src = INV_CLK_PLL;
yihui 0:972f3778c19c 2672 mpu_set_gyro_fsr(gyro_fsr);
yihui 0:972f3778c19c 2673 mpu_set_accel_fsr(accel_fsr);
yihui 0:972f3778c19c 2674 mpu_set_lpf(lpf);
yihui 0:972f3778c19c 2675 mpu_set_sample_rate(sample_rate);
yihui 0:972f3778c19c 2676 mpu_set_sensors(sensors_on);
yihui 0:972f3778c19c 2677 mpu_configure_fifo(fifo_sensors);
yihui 0:972f3778c19c 2678
yihui 0:972f3778c19c 2679 if (dmp_was_on)
yihui 0:972f3778c19c 2680 mpu_set_dmp_state(1);
yihui 0:972f3778c19c 2681
yihui 0:972f3778c19c 2682 return result;
yihui 0:972f3778c19c 2683 }
yihui 0:972f3778c19c 2684 #endif
yihui 0:972f3778c19c 2685 /*
yihui 0:972f3778c19c 2686 * \n This function must be called with the device either face-up or face-down
yihui 0:972f3778c19c 2687 * (z-axis is parallel to gravity).
yihui 0:972f3778c19c 2688 * @param[out] gyro Gyro biases in q16 format.
yihui 0:972f3778c19c 2689 * @param[out] accel Accel biases (if applicable) in q16 format.
yihui 0:972f3778c19c 2690 * @return Result mask (see above).
yihui 0:972f3778c19c 2691 */
yihui 0:972f3778c19c 2692 int mpu_run_self_test(long *gyro, long *accel)
yihui 0:972f3778c19c 2693 {
yihui 0:972f3778c19c 2694 #ifdef MPU6050
yihui 0:972f3778c19c 2695 const unsigned char tries = 2;
yihui 0:972f3778c19c 2696 long gyro_st[3], accel_st[3];
yihui 0:972f3778c19c 2697 unsigned char accel_result, gyro_result;
yihui 0:972f3778c19c 2698 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2699 unsigned char compass_result;
yihui 0:972f3778c19c 2700 #endif
yihui 0:972f3778c19c 2701 int ii;
yihui 0:972f3778c19c 2702 #endif
yihui 0:972f3778c19c 2703 int result;
yihui 0:972f3778c19c 2704 unsigned char accel_fsr, fifo_sensors, sensors_on;
yihui 0:972f3778c19c 2705 unsigned short gyro_fsr, sample_rate, lpf;
yihui 0:972f3778c19c 2706 unsigned char dmp_was_on;
yihui 0:972f3778c19c 2707
yihui 0:972f3778c19c 2708 if (st.chip_cfg.dmp_on) {
yihui 0:972f3778c19c 2709 mpu_set_dmp_state(0);
yihui 0:972f3778c19c 2710 dmp_was_on = 1;
yihui 0:972f3778c19c 2711 } else
yihui 0:972f3778c19c 2712 dmp_was_on = 0;
yihui 0:972f3778c19c 2713
yihui 0:972f3778c19c 2714 /* Get initial settings. */
yihui 0:972f3778c19c 2715 mpu_get_gyro_fsr(&gyro_fsr);
yihui 0:972f3778c19c 2716 mpu_get_accel_fsr(&accel_fsr);
yihui 0:972f3778c19c 2717 mpu_get_lpf(&lpf);
yihui 0:972f3778c19c 2718 mpu_get_sample_rate(&sample_rate);
yihui 0:972f3778c19c 2719 sensors_on = st.chip_cfg.sensors;
yihui 0:972f3778c19c 2720 mpu_get_fifo_config(&fifo_sensors);
yihui 0:972f3778c19c 2721
yihui 0:972f3778c19c 2722 /* For older chips, the self-test will be different. */
yihui 0:972f3778c19c 2723 #if defined MPU6050
yihui 0:972f3778c19c 2724 for (ii = 0; ii < tries; ii++)
yihui 0:972f3778c19c 2725 if (!get_st_biases(gyro, accel, 0))
yihui 0:972f3778c19c 2726 break;
yihui 0:972f3778c19c 2727 if (ii == tries) {
yihui 0:972f3778c19c 2728 /* If we reach this point, we most likely encountered an I2C error.
yihui 0:972f3778c19c 2729 * We'll just report an error for all three sensors.
yihui 0:972f3778c19c 2730 */
yihui 0:972f3778c19c 2731 result = 0;
yihui 0:972f3778c19c 2732 goto restore;
yihui 0:972f3778c19c 2733 }
yihui 0:972f3778c19c 2734 for (ii = 0; ii < tries; ii++)
yihui 0:972f3778c19c 2735 if (!get_st_biases(gyro_st, accel_st, 1))
yihui 0:972f3778c19c 2736 break;
yihui 0:972f3778c19c 2737 if (ii == tries) {
yihui 0:972f3778c19c 2738 /* Again, probably an I2C error. */
yihui 0:972f3778c19c 2739 result = 0;
yihui 0:972f3778c19c 2740 goto restore;
yihui 0:972f3778c19c 2741 }
yihui 0:972f3778c19c 2742 accel_result = accel_self_test(accel, accel_st);
yihui 0:972f3778c19c 2743 gyro_result = gyro_self_test(gyro, gyro_st);
yihui 0:972f3778c19c 2744
yihui 0:972f3778c19c 2745 result = 0;
yihui 0:972f3778c19c 2746 if (!gyro_result)
yihui 0:972f3778c19c 2747 result |= 0x01;
yihui 0:972f3778c19c 2748 if (!accel_result)
yihui 0:972f3778c19c 2749 result |= 0x02;
yihui 0:972f3778c19c 2750
yihui 0:972f3778c19c 2751 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2752 compass_result = compass_self_test();
yihui 0:972f3778c19c 2753 if (!compass_result)
yihui 0:972f3778c19c 2754 result |= 0x04;
yihui 0:972f3778c19c 2755 #else
yihui 0:972f3778c19c 2756 result |= 0x04;
yihui 0:972f3778c19c 2757 #endif
yihui 0:972f3778c19c 2758 restore:
yihui 0:972f3778c19c 2759 #elif defined MPU6500
yihui 0:972f3778c19c 2760 /* For now, this function will return a "pass" result for all three sensors
yihui 0:972f3778c19c 2761 * for compatibility with current test applications.
yihui 0:972f3778c19c 2762 */
yihui 0:972f3778c19c 2763 get_st_biases(gyro, accel, 0);
yihui 0:972f3778c19c 2764 result = 0x7;
yihui 0:972f3778c19c 2765 #endif
yihui 0:972f3778c19c 2766 /* Set to invalid values to ensure no I2C writes are skipped. */
yihui 0:972f3778c19c 2767 st.chip_cfg.gyro_fsr = 0xFF;
yihui 0:972f3778c19c 2768 st.chip_cfg.accel_fsr = 0xFF;
yihui 0:972f3778c19c 2769 st.chip_cfg.lpf = 0xFF;
yihui 0:972f3778c19c 2770 st.chip_cfg.sample_rate = 0xFFFF;
yihui 0:972f3778c19c 2771 st.chip_cfg.sensors = 0xFF;
yihui 0:972f3778c19c 2772 st.chip_cfg.fifo_enable = 0xFF;
yihui 0:972f3778c19c 2773 st.chip_cfg.clk_src = INV_CLK_PLL;
yihui 0:972f3778c19c 2774 mpu_set_gyro_fsr(gyro_fsr);
yihui 0:972f3778c19c 2775 mpu_set_accel_fsr(accel_fsr);
yihui 0:972f3778c19c 2776 mpu_set_lpf(lpf);
yihui 0:972f3778c19c 2777 mpu_set_sample_rate(sample_rate);
yihui 0:972f3778c19c 2778 mpu_set_sensors(sensors_on);
yihui 0:972f3778c19c 2779 mpu_configure_fifo(fifo_sensors);
yihui 0:972f3778c19c 2780
yihui 0:972f3778c19c 2781 if (dmp_was_on)
yihui 0:972f3778c19c 2782 mpu_set_dmp_state(1);
yihui 0:972f3778c19c 2783
yihui 0:972f3778c19c 2784 return result;
yihui 0:972f3778c19c 2785 }
yihui 0:972f3778c19c 2786
yihui 0:972f3778c19c 2787 /**
yihui 0:972f3778c19c 2788 * @brief Write to the DMP memory.
yihui 0:972f3778c19c 2789 * This function prevents I2C writes past the bank boundaries. The DMP memory
yihui 0:972f3778c19c 2790 * is only accessible when the chip is awake.
yihui 0:972f3778c19c 2791 * @param[in] mem_addr Memory location (bank << 8 | start address)
yihui 0:972f3778c19c 2792 * @param[in] length Number of bytes to write.
yihui 0:972f3778c19c 2793 * @param[in] data Bytes to write to memory.
yihui 0:972f3778c19c 2794 * @return 0 if successful.
yihui 0:972f3778c19c 2795 */
yihui 0:972f3778c19c 2796 int mpu_write_mem(unsigned short mem_addr, unsigned short length,
yihui 0:972f3778c19c 2797 unsigned char *data)
yihui 0:972f3778c19c 2798 {
yihui 0:972f3778c19c 2799 unsigned char tmp[2];
yihui 0:972f3778c19c 2800
yihui 0:972f3778c19c 2801 if (!data)
yihui 0:972f3778c19c 2802 return -1;
yihui 0:972f3778c19c 2803 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 2804 return -1;
yihui 0:972f3778c19c 2805
yihui 0:972f3778c19c 2806 tmp[0] = (unsigned char)(mem_addr >> 8);
yihui 0:972f3778c19c 2807 tmp[1] = (unsigned char)(mem_addr & 0xFF);
yihui 0:972f3778c19c 2808
yihui 0:972f3778c19c 2809 /* Check bank boundaries. */
yihui 0:972f3778c19c 2810 if (tmp[1] + length > st.hw->bank_size)
yihui 0:972f3778c19c 2811 return -1;
yihui 0:972f3778c19c 2812
yihui 0:972f3778c19c 2813 if (mpu_hal_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
yihui 0:972f3778c19c 2814 return -1;
yihui 0:972f3778c19c 2815 if (mpu_hal_write(st.hw->addr, st.reg->mem_r_w, length, data))
yihui 0:972f3778c19c 2816 return -1;
yihui 0:972f3778c19c 2817 return 0;
yihui 0:972f3778c19c 2818 }
yihui 0:972f3778c19c 2819
yihui 0:972f3778c19c 2820 /**
yihui 0:972f3778c19c 2821 * @brief Read from the DMP memory.
yihui 0:972f3778c19c 2822 * This function prevents I2C reads past the bank boundaries. The DMP memory
yihui 0:972f3778c19c 2823 * is only accessible when the chip is awake.
yihui 0:972f3778c19c 2824 * @param[in] mem_addr Memory location (bank << 8 | start address)
yihui 0:972f3778c19c 2825 * @param[in] length Number of bytes to read.
yihui 0:972f3778c19c 2826 * @param[out] data Bytes read from memory.
yihui 0:972f3778c19c 2827 * @return 0 if successful.
yihui 0:972f3778c19c 2828 */
yihui 0:972f3778c19c 2829 int mpu_read_mem(unsigned short mem_addr, unsigned short length,
yihui 0:972f3778c19c 2830 unsigned char *data)
yihui 0:972f3778c19c 2831 {
yihui 0:972f3778c19c 2832 unsigned char tmp[2];
yihui 0:972f3778c19c 2833
yihui 0:972f3778c19c 2834 if (!data)
yihui 0:972f3778c19c 2835 return -1;
yihui 0:972f3778c19c 2836 if (!st.chip_cfg.sensors)
yihui 0:972f3778c19c 2837 return -1;
yihui 0:972f3778c19c 2838
yihui 0:972f3778c19c 2839 tmp[0] = (unsigned char)(mem_addr >> 8);
yihui 0:972f3778c19c 2840 tmp[1] = (unsigned char)(mem_addr & 0xFF);
yihui 0:972f3778c19c 2841
yihui 0:972f3778c19c 2842 /* Check bank boundaries. */
yihui 0:972f3778c19c 2843 if (tmp[1] + length > st.hw->bank_size)
yihui 0:972f3778c19c 2844 return -1;
yihui 0:972f3778c19c 2845
yihui 0:972f3778c19c 2846 if (mpu_hal_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
yihui 0:972f3778c19c 2847 return -1;
yihui 0:972f3778c19c 2848 if (mpu_hal_read(st.hw->addr, st.reg->mem_r_w, length, data))
yihui 0:972f3778c19c 2849 return -1;
yihui 0:972f3778c19c 2850 return 0;
yihui 0:972f3778c19c 2851 }
yihui 0:972f3778c19c 2852
yihui 0:972f3778c19c 2853 /**
yihui 0:972f3778c19c 2854 * @brief Load and verify DMP image.
yihui 0:972f3778c19c 2855 * @param[in] length Length of DMP image.
yihui 0:972f3778c19c 2856 * @param[in] firmware DMP code.
yihui 0:972f3778c19c 2857 * @param[in] start_addr Starting address of DMP code memory.
yihui 0:972f3778c19c 2858 * @param[in] sample_rate Fixed sampling rate used when DMP is enabled.
yihui 0:972f3778c19c 2859 * @return 0 if successful.
yihui 0:972f3778c19c 2860 */
yihui 0:972f3778c19c 2861 int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
yihui 0:972f3778c19c 2862 unsigned short start_addr, unsigned short sample_rate)
yihui 0:972f3778c19c 2863 {
yihui 0:972f3778c19c 2864 unsigned short ii;
yihui 0:972f3778c19c 2865 unsigned short this_write;
yihui 0:972f3778c19c 2866 /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
yihui 0:972f3778c19c 2867 #define LOAD_CHUNK (16)
yihui 0:972f3778c19c 2868 static unsigned char cur[LOAD_CHUNK], tmp[2];
yihui 0:972f3778c19c 2869
yihui 0:972f3778c19c 2870 if (st.chip_cfg.dmp_loaded)
yihui 0:972f3778c19c 2871 /* DMP should only be loaded once. */
yihui 0:972f3778c19c 2872 return -1;
yihui 0:972f3778c19c 2873
yihui 0:972f3778c19c 2874 if (!firmware)
yihui 0:972f3778c19c 2875 return -1;
yihui 0:972f3778c19c 2876 for (ii = 0; ii < length; ii += this_write) {
yihui 0:972f3778c19c 2877 this_write = min(LOAD_CHUNK, length - ii);
yihui 0:972f3778c19c 2878 if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
yihui 0:972f3778c19c 2879 return -1;
yihui 0:972f3778c19c 2880 if (mpu_read_mem(ii, this_write, cur))
yihui 0:972f3778c19c 2881 return -1;
yihui 0:972f3778c19c 2882 if (memcmp(firmware+ii, cur, this_write))
yihui 0:972f3778c19c 2883 return -2;
yihui 0:972f3778c19c 2884 }
yihui 0:972f3778c19c 2885
yihui 0:972f3778c19c 2886 /* Set program start address. */
yihui 0:972f3778c19c 2887 tmp[0] = start_addr >> 8;
yihui 0:972f3778c19c 2888 tmp[1] = start_addr & 0xFF;
yihui 0:972f3778c19c 2889 if (mpu_hal_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
yihui 0:972f3778c19c 2890 return -1;
yihui 0:972f3778c19c 2891
yihui 0:972f3778c19c 2892 st.chip_cfg.dmp_loaded = 1;
yihui 0:972f3778c19c 2893 st.chip_cfg.dmp_sample_rate = sample_rate;
yihui 0:972f3778c19c 2894 return 0;
yihui 0:972f3778c19c 2895 }
yihui 0:972f3778c19c 2896
yihui 0:972f3778c19c 2897 /**
yihui 0:972f3778c19c 2898 * @brief Enable/disable DMP support.
yihui 0:972f3778c19c 2899 * @param[in] enable 1 to turn on the DMP.
yihui 0:972f3778c19c 2900 * @return 0 if successful.
yihui 0:972f3778c19c 2901 */
yihui 0:972f3778c19c 2902 int mpu_set_dmp_state(unsigned char enable)
yihui 0:972f3778c19c 2903 {
yihui 0:972f3778c19c 2904 unsigned char tmp;
yihui 0:972f3778c19c 2905 if (st.chip_cfg.dmp_on == enable)
yihui 0:972f3778c19c 2906 return 0;
yihui 0:972f3778c19c 2907
yihui 0:972f3778c19c 2908 if (enable) {
yihui 0:972f3778c19c 2909 if (!st.chip_cfg.dmp_loaded)
yihui 0:972f3778c19c 2910 return -1;
yihui 0:972f3778c19c 2911 /* Disable data ready interrupt. */
yihui 0:972f3778c19c 2912 set_int_enable(0);
yihui 0:972f3778c19c 2913 /* Disable bypass mode. */
yihui 0:972f3778c19c 2914 mpu_set_bypass(0);
yihui 0:972f3778c19c 2915 /* Keep constant sample rate, FIFO rate controlled by DMP. */
yihui 0:972f3778c19c 2916 mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
yihui 0:972f3778c19c 2917 /* Remove FIFO elements. */
yihui 0:972f3778c19c 2918 tmp = 0;
yihui 0:972f3778c19c 2919 mpu_hal_write(st.hw->addr, 0x23, 1, &tmp);
yihui 0:972f3778c19c 2920 st.chip_cfg.dmp_on = 1;
yihui 0:972f3778c19c 2921 /* Enable DMP interrupt. */
yihui 0:972f3778c19c 2922 set_int_enable(1);
yihui 0:972f3778c19c 2923 mpu_reset_fifo();
yihui 0:972f3778c19c 2924 } else {
yihui 0:972f3778c19c 2925 /* Disable DMP interrupt. */
yihui 0:972f3778c19c 2926 set_int_enable(0);
yihui 0:972f3778c19c 2927 /* Restore FIFO settings. */
yihui 0:972f3778c19c 2928 tmp = st.chip_cfg.fifo_enable;
yihui 0:972f3778c19c 2929 mpu_hal_write(st.hw->addr, 0x23, 1, &tmp);
yihui 0:972f3778c19c 2930 st.chip_cfg.dmp_on = 0;
yihui 0:972f3778c19c 2931 mpu_reset_fifo();
yihui 0:972f3778c19c 2932 }
yihui 0:972f3778c19c 2933 return 0;
yihui 0:972f3778c19c 2934 }
yihui 0:972f3778c19c 2935
yihui 0:972f3778c19c 2936 /**
yihui 0:972f3778c19c 2937 * @brief Get DMP state.
yihui 0:972f3778c19c 2938 * @param[out] enabled 1 if enabled.
yihui 0:972f3778c19c 2939 * @return 0 if successful.
yihui 0:972f3778c19c 2940 */
yihui 0:972f3778c19c 2941 int mpu_get_dmp_state(unsigned char *enabled)
yihui 0:972f3778c19c 2942 {
yihui 0:972f3778c19c 2943 enabled[0] = st.chip_cfg.dmp_on;
yihui 0:972f3778c19c 2944 return 0;
yihui 0:972f3778c19c 2945 }
yihui 0:972f3778c19c 2946
yihui 0:972f3778c19c 2947 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 2948 /* This initialization is similar to the one in ak8975.c. */
yihui 0:972f3778c19c 2949 static int setup_compass(void)
yihui 0:972f3778c19c 2950 {
yihui 0:972f3778c19c 2951 unsigned char data[4], akm_addr;
yihui 0:972f3778c19c 2952
yihui 0:972f3778c19c 2953 #ifndef MPU_USE_SPI
yihui 0:972f3778c19c 2954 mpu_set_bypass(1);
yihui 0:972f3778c19c 2955
yihui 0:972f3778c19c 2956 /* Find compass. Possible addresses range from 0x0C to 0x0F. */
yihui 0:972f3778c19c 2957 for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
yihui 0:972f3778c19c 2958 int result;
yihui 0:972f3778c19c 2959 result = mpu_hal_read(akm_addr, AKM_REG_WHOAMI, 1, data);
yihui 0:972f3778c19c 2960 if (!result && (data[0] == AKM_WHOAMI))
yihui 0:972f3778c19c 2961 break;
yihui 0:972f3778c19c 2962 }
yihui 0:972f3778c19c 2963
yihui 0:972f3778c19c 2964 if (akm_addr > 0x0F) {
yihui 0:972f3778c19c 2965 /* TODO: Handle this case in all compass-related functions. */
yihui 0:972f3778c19c 2966 log_e("Compass not found.\n");
yihui 0:972f3778c19c 2967 return -1;
yihui 0:972f3778c19c 2968 }
yihui 0:972f3778c19c 2969
yihui 0:972f3778c19c 2970 st.chip_cfg.compass_addr = akm_addr;
yihui 0:972f3778c19c 2971
yihui 0:972f3778c19c 2972 data[0] = AKM_POWER_DOWN;
yihui 0:972f3778c19c 2973 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
yihui 0:972f3778c19c 2974 return -1;
yihui 0:972f3778c19c 2975 delay_ms(1);
yihui 0:972f3778c19c 2976
yihui 0:972f3778c19c 2977 data[0] = AKM_FUSE_ROM_ACCESS;
yihui 0:972f3778c19c 2978 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
yihui 0:972f3778c19c 2979 return -1;
yihui 0:972f3778c19c 2980 delay_ms(1);
yihui 0:972f3778c19c 2981
yihui 0:972f3778c19c 2982 /* Get sensitivity adjustment data from fuse ROM. */
yihui 0:972f3778c19c 2983 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
yihui 0:972f3778c19c 2984 return -1;
yihui 0:972f3778c19c 2985 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
yihui 0:972f3778c19c 2986 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
yihui 0:972f3778c19c 2987 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
yihui 0:972f3778c19c 2988
yihui 0:972f3778c19c 2989 data[0] = AKM_POWER_DOWN;
yihui 0:972f3778c19c 2990 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
yihui 0:972f3778c19c 2991 return -1;
yihui 0:972f3778c19c 2992 delay_ms(1);
yihui 0:972f3778c19c 2993
yihui 0:972f3778c19c 2994 mpu_set_bypass(0);
yihui 0:972f3778c19c 2995
yihui 0:972f3778c19c 2996 /* Set up master mode, master clock, and ES bit. */
yihui 0:972f3778c19c 2997 data[0] = 0x40;
yihui 0:972f3778c19c 2998 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data))
yihui 0:972f3778c19c 2999 return -1;
yihui 0:972f3778c19c 3000
yihui 0:972f3778c19c 3001 #else
yihui 0:972f3778c19c 3002 akm_addr = 0x0C;
yihui 0:972f3778c19c 3003 st.chip_cfg.compass_addr = akm_addr;
yihui 0:972f3778c19c 3004
yihui 0:972f3778c19c 3005 /* Set up master mode, master clock, and ES bit. */
yihui 0:972f3778c19c 3006 data[0] = 0x40;
yihui 0:972f3778c19c 3007 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data))
yihui 0:972f3778c19c 3008 return -1;
yihui 0:972f3778c19c 3009
yihui 0:972f3778c19c 3010 /* Slave 0 reads from AKM adj registers. */
yihui 0:972f3778c19c 3011 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
yihui 0:972f3778c19c 3012 if (mpu_hal_write(st.hw->addr, st.reg->s0_addr, 1, data))
yihui 0:972f3778c19c 3013 return -1;
yihui 0:972f3778c19c 3014
yihui 0:972f3778c19c 3015 /* Compass reads start at this register. */
yihui 0:972f3778c19c 3016 data[0] = AKM_REG_ASAX;
yihui 0:972f3778c19c 3017 if (mpu_hal_write(st.hw->addr, st.reg->s0_reg, 1, data))
yihui 0:972f3778c19c 3018 return -1;
yihui 0:972f3778c19c 3019
yihui 0:972f3778c19c 3020 /* Enable slave 0, 8-byte reads. */
yihui 0:972f3778c19c 3021 data[0] = BIT_SLAVE_EN | 3;
yihui 0:972f3778c19c 3022 if (mpu_hal_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
yihui 0:972f3778c19c 3023 return -1;
yihui 0:972f3778c19c 3024
yihui 0:972f3778c19c 3025 if (mpu_hal_read(st.hw->addr, st.reg->raw_compass, 3, data))
yihui 0:972f3778c19c 3026 return -1;
yihui 0:972f3778c19c 3027
yihui 0:972f3778c19c 3028 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
yihui 0:972f3778c19c 3029 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
yihui 0:972f3778c19c 3030 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
yihui 0:972f3778c19c 3031
yihui 0:972f3778c19c 3032 #endif
yihui 0:972f3778c19c 3033
yihui 0:972f3778c19c 3034 /* Slave 0 reads from AKM data registers. */
yihui 0:972f3778c19c 3035 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
yihui 0:972f3778c19c 3036 if (mpu_hal_write(st.hw->addr, st.reg->s0_addr, 1, data))
yihui 0:972f3778c19c 3037 return -1;
yihui 0:972f3778c19c 3038
yihui 0:972f3778c19c 3039 /* Compass reads start at this register. */
yihui 0:972f3778c19c 3040 data[0] = AKM_REG_ST1;
yihui 0:972f3778c19c 3041 if (mpu_hal_write(st.hw->addr, st.reg->s0_reg, 1, data))
yihui 0:972f3778c19c 3042 return -1;
yihui 0:972f3778c19c 3043
yihui 0:972f3778c19c 3044 /* Enable slave 0, 8-byte reads. */
yihui 0:972f3778c19c 3045 data[0] = BIT_SLAVE_EN | 8;
yihui 0:972f3778c19c 3046 if (mpu_hal_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
yihui 0:972f3778c19c 3047 return -1;
yihui 0:972f3778c19c 3048
yihui 0:972f3778c19c 3049 /* Slave 1 changes AKM measurement mode. */
yihui 0:972f3778c19c 3050 data[0] = st.chip_cfg.compass_addr;
yihui 0:972f3778c19c 3051 if (mpu_hal_write(st.hw->addr, st.reg->s1_addr, 1, data))
yihui 0:972f3778c19c 3052 return -1;
yihui 0:972f3778c19c 3053
yihui 0:972f3778c19c 3054 /* AKM measurement mode register. */
yihui 0:972f3778c19c 3055 data[0] = AKM_REG_CNTL;
yihui 0:972f3778c19c 3056 if (mpu_hal_write(st.hw->addr, st.reg->s1_reg, 1, data))
yihui 0:972f3778c19c 3057 return -1;
yihui 0:972f3778c19c 3058
yihui 0:972f3778c19c 3059 /* Enable slave 1, 1-byte writes. */
yihui 0:972f3778c19c 3060 data[0] = BIT_SLAVE_EN | 1;
yihui 0:972f3778c19c 3061 if (mpu_hal_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
yihui 0:972f3778c19c 3062 return -1;
yihui 0:972f3778c19c 3063
yihui 0:972f3778c19c 3064 /* Set slave 1 data. */
yihui 0:972f3778c19c 3065 data[0] = AKM_SINGLE_MEASUREMENT;
yihui 0:972f3778c19c 3066 if (mpu_hal_write(st.hw->addr, st.reg->s1_do, 1, data))
yihui 0:972f3778c19c 3067 return -1;
yihui 0:972f3778c19c 3068
yihui 0:972f3778c19c 3069 /* Trigger slave 0 and slave 1 actions at each sample. */
yihui 0:972f3778c19c 3070 data[0] = 0x03;
yihui 0:972f3778c19c 3071 if (mpu_hal_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
yihui 0:972f3778c19c 3072 return -1;
yihui 0:972f3778c19c 3073
yihui 0:972f3778c19c 3074 #ifdef MPU9150
yihui 0:972f3778c19c 3075 /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
yihui 0:972f3778c19c 3076 data[0] = BIT_I2C_MST_VDDIO;
yihui 0:972f3778c19c 3077 if (mpu_hal_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
yihui 0:972f3778c19c 3078 return -1;
yihui 0:972f3778c19c 3079 #endif
yihui 0:972f3778c19c 3080
yihui 0:972f3778c19c 3081 return 0;
yihui 0:972f3778c19c 3082 }
yihui 0:972f3778c19c 3083 #endif
yihui 0:972f3778c19c 3084
yihui 0:972f3778c19c 3085 /**
yihui 0:972f3778c19c 3086 * @brief Read raw compass data.
yihui 0:972f3778c19c 3087 * @param[out] data Raw data in hardware units.
yihui 0:972f3778c19c 3088 * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
yihui 0:972f3778c19c 3089 * @return 0 if successful.
yihui 0:972f3778c19c 3090 */
yihui 0:972f3778c19c 3091 int mpu_get_compass_reg(short *data, unsigned long *timestamp)
yihui 0:972f3778c19c 3092 {
yihui 0:972f3778c19c 3093 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 3094 unsigned char tmp[9];
yihui 0:972f3778c19c 3095
yihui 0:972f3778c19c 3096 if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
yihui 0:972f3778c19c 3097 return -1;
yihui 0:972f3778c19c 3098
yihui 0:972f3778c19c 3099 #ifdef AK89xx_BYPASS
yihui 0:972f3778c19c 3100 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
yihui 0:972f3778c19c 3101 return -1;
yihui 0:972f3778c19c 3102 tmp[8] = AKM_SINGLE_MEASUREMENT;
yihui 0:972f3778c19c 3103 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
yihui 0:972f3778c19c 3104 return -1;
yihui 0:972f3778c19c 3105 #else
yihui 0:972f3778c19c 3106 if (mpu_hal_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
yihui 0:972f3778c19c 3107 return -1;
yihui 0:972f3778c19c 3108 #endif
yihui 0:972f3778c19c 3109
yihui 0:972f3778c19c 3110 #if defined AK8975_SECONDARY
yihui 0:972f3778c19c 3111 /* AK8975 doesn't have the overrun error bit. */
yihui 0:972f3778c19c 3112 if (!(tmp[0] & AKM_DATA_READY))
yihui 0:972f3778c19c 3113 return -2;
yihui 0:972f3778c19c 3114 if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
yihui 0:972f3778c19c 3115 return -3;
yihui 0:972f3778c19c 3116 #elif defined AK8963_SECONDARY
yihui 0:972f3778c19c 3117 /* AK8963 doesn't have the data read error bit. */
yihui 0:972f3778c19c 3118 if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
yihui 0:972f3778c19c 3119 return -2;
yihui 0:972f3778c19c 3120 if (tmp[7] & AKM_OVERFLOW)
yihui 0:972f3778c19c 3121 return -3;
yihui 0:972f3778c19c 3122 #endif
yihui 0:972f3778c19c 3123 data[0] = (tmp[2] << 8) | tmp[1];
yihui 0:972f3778c19c 3124 data[1] = (tmp[4] << 8) | tmp[3];
yihui 0:972f3778c19c 3125 data[2] = (tmp[6] << 8) | tmp[5];
yihui 0:972f3778c19c 3126
yihui 0:972f3778c19c 3127 data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
yihui 0:972f3778c19c 3128 data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
yihui 0:972f3778c19c 3129 data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
yihui 0:972f3778c19c 3130
yihui 0:972f3778c19c 3131 if (timestamp)
yihui 0:972f3778c19c 3132 get_ms(timestamp);
yihui 0:972f3778c19c 3133 return 0;
yihui 0:972f3778c19c 3134 #else
yihui 0:972f3778c19c 3135 return -1;
yihui 0:972f3778c19c 3136 #endif
yihui 0:972f3778c19c 3137 }
yihui 0:972f3778c19c 3138
yihui 0:972f3778c19c 3139 /**
yihui 0:972f3778c19c 3140 * @brief Get the compass full-scale range.
yihui 0:972f3778c19c 3141 * @param[out] fsr Current full-scale range.
yihui 0:972f3778c19c 3142 * @return 0 if successful.
yihui 0:972f3778c19c 3143 */
yihui 0:972f3778c19c 3144 int mpu_get_compass_fsr(unsigned short *fsr)
yihui 0:972f3778c19c 3145 {
yihui 0:972f3778c19c 3146 #ifdef AK89xx_SECONDARY
yihui 0:972f3778c19c 3147 fsr[0] = st.hw->compass_fsr;
yihui 0:972f3778c19c 3148 return 0;
yihui 0:972f3778c19c 3149 #else
yihui 0:972f3778c19c 3150 return -1;
yihui 0:972f3778c19c 3151 #endif
yihui 0:972f3778c19c 3152 }
yihui 0:972f3778c19c 3153
yihui 0:972f3778c19c 3154 /**
yihui 0:972f3778c19c 3155 * @brief Enters LP accel motion interrupt mode.
yihui 0:972f3778c19c 3156 * The behaviour of this feature is very different between the MPU6050 and the
yihui 0:972f3778c19c 3157 * MPU6500. Each chip's version of this feature is explained below.
yihui 0:972f3778c19c 3158 *
yihui 0:972f3778c19c 3159 * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
yihui 0:972f3778c19c 3160 * increments.
yihui 0:972f3778c19c 3161 *
yihui 0:972f3778c19c 3162 * \n Low-power accel mode supports the following frequencies:
yihui 0:972f3778c19c 3163 * \n 1.25Hz, 5Hz, 20Hz, 40Hz
yihui 0:972f3778c19c 3164 *
yihui 0:972f3778c19c 3165 * \n MPU6500:
yihui 0:972f3778c19c 3166 * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
yihui 0:972f3778c19c 3167 * sample. The hardware monitors the accel data and detects any large change
yihui 0:972f3778c19c 3168 * over a short period of time.
yihui 0:972f3778c19c 3169 *
yihui 0:972f3778c19c 3170 * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
yihui 0:972f3778c19c 3171 * increments.
yihui 0:972f3778c19c 3172 *
yihui 0:972f3778c19c 3173 * \n MPU6500 Low-power accel mode supports the following frequencies:
yihui 0:972f3778c19c 3174 * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
yihui 0:972f3778c19c 3175 *
yihui 0:972f3778c19c 3176 * \n\n NOTES:
yihui 0:972f3778c19c 3177 * \n The driver will round down @e thresh to the nearest supported value if
yihui 0:972f3778c19c 3178 * an unsupported threshold is selected.
yihui 0:972f3778c19c 3179 * \n To select a fractional wake-up frequency, round down the value passed to
yihui 0:972f3778c19c 3180 * @e lpa_freq.
yihui 0:972f3778c19c 3181 * \n The MPU6500 does not support a delay parameter. If this function is used
yihui 0:972f3778c19c 3182 * for the MPU6500, the value passed to @e time will be ignored.
yihui 0:972f3778c19c 3183 * \n To disable this mode, set @e lpa_freq to zero. The driver will restore
yihui 0:972f3778c19c 3184 * the previous configuration.
yihui 0:972f3778c19c 3185 *
yihui 0:972f3778c19c 3186 * @param[in] thresh Motion threshold in mg.
yihui 0:972f3778c19c 3187 * @param[in] time Duration in milliseconds that the accel data must
yihui 0:972f3778c19c 3188 * exceed @e thresh before motion is reported.
yihui 0:972f3778c19c 3189 * @param[in] lpa_freq Minimum sampling rate, or zero to disable.
yihui 0:972f3778c19c 3190 * @return 0 if successful.
yihui 0:972f3778c19c 3191 */
yihui 0:972f3778c19c 3192 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
yihui 0:972f3778c19c 3193 unsigned char lpa_freq)
yihui 0:972f3778c19c 3194 {
yihui 0:972f3778c19c 3195
yihui 0:972f3778c19c 3196 #if defined MPU6500
yihui 0:972f3778c19c 3197 unsigned char data[3];
yihui 0:972f3778c19c 3198 #endif
yihui 0:972f3778c19c 3199 if (lpa_freq) {
yihui 0:972f3778c19c 3200 #if defined MPU6500
yihui 0:972f3778c19c 3201 unsigned char thresh_hw;
yihui 0:972f3778c19c 3202
yihui 0:972f3778c19c 3203 /* 1LSb = 4mg. */
yihui 0:972f3778c19c 3204 if (thresh > 1020)
yihui 0:972f3778c19c 3205 thresh_hw = 255;
yihui 0:972f3778c19c 3206 else if (thresh < 4)
yihui 0:972f3778c19c 3207 thresh_hw = 1;
yihui 0:972f3778c19c 3208 else
yihui 0:972f3778c19c 3209 thresh_hw = thresh >> 2;
yihui 0:972f3778c19c 3210 #endif
yihui 0:972f3778c19c 3211
yihui 0:972f3778c19c 3212 if (!time)
yihui 0:972f3778c19c 3213 /* Minimum duration must be 1ms. */
yihui 0:972f3778c19c 3214 time = 1;
yihui 0:972f3778c19c 3215
yihui 0:972f3778c19c 3216 #if defined MPU6500
yihui 0:972f3778c19c 3217 if (lpa_freq > 640)
yihui 0:972f3778c19c 3218 /* At this point, the chip has not been re-configured, so the
yihui 0:972f3778c19c 3219 * function can safely exit.
yihui 0:972f3778c19c 3220 */
yihui 0:972f3778c19c 3221 return -1;
yihui 0:972f3778c19c 3222 #endif
yihui 0:972f3778c19c 3223
yihui 0:972f3778c19c 3224 if (!st.chip_cfg.int_motion_only) {
yihui 0:972f3778c19c 3225 /* Store current settings for later. */
yihui 0:972f3778c19c 3226 if (st.chip_cfg.dmp_on) {
yihui 0:972f3778c19c 3227 mpu_set_dmp_state(0);
yihui 0:972f3778c19c 3228 st.chip_cfg.cache.dmp_on = 1;
yihui 0:972f3778c19c 3229 } else
yihui 0:972f3778c19c 3230 st.chip_cfg.cache.dmp_on = 0;
yihui 0:972f3778c19c 3231 mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
yihui 0:972f3778c19c 3232 mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
yihui 0:972f3778c19c 3233 mpu_get_lpf(&st.chip_cfg.cache.lpf);
yihui 0:972f3778c19c 3234 mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
yihui 0:972f3778c19c 3235 st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
yihui 0:972f3778c19c 3236 mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
yihui 0:972f3778c19c 3237 }
yihui 0:972f3778c19c 3238
yihui 0:972f3778c19c 3239 #if defined MPU6500
yihui 0:972f3778c19c 3240 /* Disable hardware interrupts. */
yihui 0:972f3778c19c 3241 set_int_enable(0);
yihui 0:972f3778c19c 3242
yihui 0:972f3778c19c 3243 /* Enter full-power accel-only mode, no FIFO/DMP. */
yihui 0:972f3778c19c 3244 data[0] = 0;
yihui 0:972f3778c19c 3245 data[1] = 0;
yihui 0:972f3778c19c 3246 data[2] = BIT_STBY_XYZG;
yihui 0:972f3778c19c 3247 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 3, data))
yihui 0:972f3778c19c 3248 goto lp_int_restore;
yihui 0:972f3778c19c 3249
yihui 0:972f3778c19c 3250 /* Set motion threshold. */
yihui 0:972f3778c19c 3251 data[0] = thresh_hw;
yihui 0:972f3778c19c 3252 if (mpu_hal_write(st.hw->addr, st.reg->motion_thr, 1, data))
yihui 0:972f3778c19c 3253 goto lp_int_restore;
yihui 0:972f3778c19c 3254
yihui 0:972f3778c19c 3255 /* Set wake frequency. */
yihui 0:972f3778c19c 3256 if (lpa_freq == 1)
yihui 0:972f3778c19c 3257 data[0] = INV_LPA_1_25HZ;
yihui 0:972f3778c19c 3258 else if (lpa_freq == 2)
yihui 0:972f3778c19c 3259 data[0] = INV_LPA_2_5HZ;
yihui 0:972f3778c19c 3260 else if (lpa_freq <= 5)
yihui 0:972f3778c19c 3261 data[0] = INV_LPA_5HZ;
yihui 0:972f3778c19c 3262 else if (lpa_freq <= 10)
yihui 0:972f3778c19c 3263 data[0] = INV_LPA_10HZ;
yihui 0:972f3778c19c 3264 else if (lpa_freq <= 20)
yihui 0:972f3778c19c 3265 data[0] = INV_LPA_20HZ;
yihui 0:972f3778c19c 3266 else if (lpa_freq <= 40)
yihui 0:972f3778c19c 3267 data[0] = INV_LPA_40HZ;
yihui 0:972f3778c19c 3268 else if (lpa_freq <= 80)
yihui 0:972f3778c19c 3269 data[0] = INV_LPA_80HZ;
yihui 0:972f3778c19c 3270 else if (lpa_freq <= 160)
yihui 0:972f3778c19c 3271 data[0] = INV_LPA_160HZ;
yihui 0:972f3778c19c 3272 else if (lpa_freq <= 320)
yihui 0:972f3778c19c 3273 data[0] = INV_LPA_320HZ;
yihui 0:972f3778c19c 3274 else
yihui 0:972f3778c19c 3275 data[0] = INV_LPA_640HZ;
yihui 0:972f3778c19c 3276 if (mpu_hal_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
yihui 0:972f3778c19c 3277 goto lp_int_restore;
yihui 0:972f3778c19c 3278
yihui 0:972f3778c19c 3279 /* Enable motion interrupt (MPU6500 version). */
yihui 0:972f3778c19c 3280 data[0] = BITS_WOM_EN;
yihui 0:972f3778c19c 3281 if (mpu_hal_write(st.hw->addr, st.reg->accel_intel, 1, data))
yihui 0:972f3778c19c 3282 goto lp_int_restore;
yihui 0:972f3778c19c 3283
yihui 0:972f3778c19c 3284 /* Enable cycle mode. */
yihui 0:972f3778c19c 3285 data[0] = BIT_LPA_CYCLE;
yihui 0:972f3778c19c 3286 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
yihui 0:972f3778c19c 3287 goto lp_int_restore;
yihui 0:972f3778c19c 3288
yihui 0:972f3778c19c 3289 /* Enable interrupt. */
yihui 0:972f3778c19c 3290 data[0] = BIT_MOT_INT_EN;
yihui 0:972f3778c19c 3291 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data))
yihui 0:972f3778c19c 3292 goto lp_int_restore;
yihui 0:972f3778c19c 3293
yihui 0:972f3778c19c 3294 st.chip_cfg.int_motion_only = 1;
yihui 0:972f3778c19c 3295 return 0;
yihui 0:972f3778c19c 3296 #endif
yihui 0:972f3778c19c 3297 } else {
yihui 0:972f3778c19c 3298 /* Don't "restore" the previous state if no state has been saved. */
yihui 0:972f3778c19c 3299 int ii;
yihui 0:972f3778c19c 3300 char *cache_ptr = (char*)&st.chip_cfg.cache;
yihui 0:972f3778c19c 3301 for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
yihui 0:972f3778c19c 3302 if (cache_ptr[ii] != 0)
yihui 0:972f3778c19c 3303 goto lp_int_restore;
yihui 0:972f3778c19c 3304 }
yihui 0:972f3778c19c 3305 /* If we reach this point, motion interrupt mode hasn't been used yet. */
yihui 0:972f3778c19c 3306 return -1;
yihui 0:972f3778c19c 3307 }
yihui 0:972f3778c19c 3308 lp_int_restore:
yihui 0:972f3778c19c 3309 /* Set to invalid values to ensure no I2C writes are skipped. */
yihui 0:972f3778c19c 3310 st.chip_cfg.gyro_fsr = 0xFF;
yihui 0:972f3778c19c 3311 st.chip_cfg.accel_fsr = 0xFF;
yihui 0:972f3778c19c 3312 st.chip_cfg.lpf = 0xFF;
yihui 0:972f3778c19c 3313 st.chip_cfg.sample_rate = 0xFFFF;
yihui 0:972f3778c19c 3314 st.chip_cfg.sensors = 0xFF;
yihui 0:972f3778c19c 3315 st.chip_cfg.fifo_enable = 0xFF;
yihui 0:972f3778c19c 3316 st.chip_cfg.clk_src = INV_CLK_PLL;
yihui 0:972f3778c19c 3317 mpu_set_sensors(st.chip_cfg.cache.sensors_on);
yihui 0:972f3778c19c 3318 mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
yihui 0:972f3778c19c 3319 mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
yihui 0:972f3778c19c 3320 mpu_set_lpf(st.chip_cfg.cache.lpf);
yihui 0:972f3778c19c 3321 mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
yihui 0:972f3778c19c 3322 mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
yihui 0:972f3778c19c 3323
yihui 0:972f3778c19c 3324 if (st.chip_cfg.cache.dmp_on)
yihui 0:972f3778c19c 3325 mpu_set_dmp_state(1);
yihui 0:972f3778c19c 3326
yihui 0:972f3778c19c 3327 #ifdef MPU6500
yihui 0:972f3778c19c 3328 /* Disable motion interrupt (MPU6500 version). */
yihui 0:972f3778c19c 3329 data[0] = 0;
yihui 0:972f3778c19c 3330 if (mpu_hal_write(st.hw->addr, st.reg->accel_intel, 1, data))
yihui 0:972f3778c19c 3331 goto lp_int_restore;
yihui 0:972f3778c19c 3332 #endif
yihui 0:972f3778c19c 3333
yihui 0:972f3778c19c 3334 st.chip_cfg.int_motion_only = 0;
yihui 0:972f3778c19c 3335 return 0;
yihui 0:972f3778c19c 3336 }
yihui 0:972f3778c19c 3337
yihui 0:972f3778c19c 3338 /**
yihui 0:972f3778c19c 3339 * @}
yihui 0:972f3778c19c 3340 */
yihui 0:972f3778c19c 3341