Filip Richter / eMPL_MPU6050

MPU6050 library

Fork of eMPL_MPU6050 by Seeed

inv_mpu.c@2:6c7eb2f19ada, 2015-11-27 (annotated)

Committer:
richterfilip
Date:
Fri Nov 27 21:12:04 2015 +0000
Revision:
2:6c7eb2f19ada
Parent:
0:1b6dab73c06b
Child:
3:b931c48531b4
Added more debug code

Who changed what in which revision?

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