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