Yihui Xiong
initial
Embed:
(wiki syntax)
Show/hide line numbers
inv_mpu.c
Go to the documentation of this file.
00001 /* 00002 $License: 00003 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 00004 See included License.txt for License information. 00005 $ 00006 */ 00007 /** 00008 * @addtogroup DRIVERS Sensor Driver Layer 00009 * @brief Hardware drivers to communicate with sensors via I2C. 00010 * 00011 * @{ 00012 * @file inv_mpu.c 00013 * @brief An I2C-based driver for Invensense gyroscopes. 00014 * @details This driver currently works for the following devices: 00015 * MPU6050 00016 * MPU6500 00017 * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) 00018 * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) 00019 */ 00020 #include <stdio.h> 00021 #include <stdint.h> 00022 #include <stdlib.h> 00023 #include <string.h> 00024 #include <math.h> 00025 #include "inv_mpu.h" 00026 00027 /* The following functions must be defined for this platform: 00028 * mpu_hal_write(unsigned char slave_addr, unsigned char reg_addr, 00029 * unsigned char length, unsigned char const *data) 00030 * mpu_hal_read(unsigned char slave_addr, unsigned char reg_addr, 00031 * unsigned char length, unsigned char *data) 00032 * delay_ms(unsigned long num_ms) 00033 * get_ms(unsigned long *count) 00034 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) 00035 * labs(long x) 00036 * fabsf(float x) 00037 * min(int a, int b) 00038 */ 00039 #if defined __MBED__ // for mbed platform 00040 #include "mpu_mbed_config.h" 00041 #elif defined MOTION_DRIVER_TARGET_MSP430 00042 #include "msp430.h" 00043 #include "msp430_i2c.h" 00044 #include "msp430_clock.h" 00045 #include "msp430_interrupt.h" 00046 #define mpu_hal_write msp430_mpu_hal_write 00047 #define mpu_hal_read msp430_mpu_hal_read 00048 #define delay_ms msp430_delay_ms 00049 #define get_ms msp430_get_clock_ms 00050 static inline int reg_int_cb(struct int_param_s *int_param) 00051 { 00052 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00053 int_param->active_low); 00054 } 00055 #define log_i(...) do {} while (0) 00056 #define log_e(...) do {} while (0) 00057 /* labs is already defined by TI's toolchain. */ 00058 /* fabs is for doubles. fabsf is for floats. */ 00059 #define fabs fabsf 00060 #define min(a,b) ((a<b)?a:b) 00061 #elif defined EMPL_TARGET_MSP430 00062 #include "msp430.h" 00063 #include "msp430_i2c.h" 00064 #include "msp430_clock.h" 00065 #include "msp430_interrupt.h" 00066 #include "log.h" 00067 #define mpu_hal_write msp430_mpu_hal_write 00068 #define mpu_hal_read msp430_mpu_hal_read 00069 #define delay_ms msp430_delay_ms 00070 #define get_ms msp430_get_clock_ms 00071 static inline int reg_int_cb(struct int_param_s *int_param) 00072 { 00073 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00074 int_param->active_low); 00075 } 00076 #define log_i MPL_LOGI 00077 #define log_e MPL_LOGE 00078 /* labs is already defined by TI's toolchain. */ 00079 /* fabs is for doubles. fabsf is for floats. */ 00080 #define fabs fabsf 00081 #define min(a,b) ((a<b)?a:b) 00082 #elif defined EMPL_TARGET_UC3L0 00083 /* Instead of using the standard TWI driver from the ASF library, we're using 00084 * a TWI driver that follows the slave address + register address convention. 00085 */ 00086 #include "twi.h" 00087 #include "delay.h" 00088 #include "sysclk.h" 00089 #include "log.h" 00090 #include "sensors_xplained.h" 00091 #include "uc3l0_clock.h" 00092 #define mpu_hal_write(a, b, c, d) twi_write(a, b, d, c) 00093 #define mpu_hal_read(a, b, c, d) twi_read(a, b, d, c) 00094 /* delay_ms is a function already defined in ASF. */ 00095 #define get_ms uc3l0_get_clock_ms 00096 static inline int reg_int_cb(struct int_param_s *int_param) 00097 { 00098 sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg); 00099 return 0; 00100 } 00101 #define log_i MPL_LOGI 00102 #define log_e MPL_LOGE 00103 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */ 00104 #define labs abs 00105 #define fabs(x) (((x)>0)?(x):-(x)) 00106 00107 #else 00108 #error Gyro driver is missing the system layer implementations. 00109 #endif 00110 00111 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 00112 #error Which gyro are you using? Define MPUxxxx in your compiler options. 00113 #endif 00114 00115 /* Time for some messy macro work. =] 00116 * #define MPU9150 00117 * is equivalent to.. 00118 * #define MPU6050 00119 * #define AK8975_SECONDARY 00120 * 00121 * #define MPU9250 00122 * is equivalent to.. 00123 * #define MPU6500 00124 * #define AK8963_SECONDARY 00125 */ 00126 #if defined MPU9150 00127 #ifndef MPU6050 00128 #define MPU6050 00129 #endif /* #ifndef MPU6050 */ 00130 #if defined AK8963_SECONDARY 00131 #error "MPU9150 and AK8963_SECONDARY cannot both be defined." 00132 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ 00133 #define AK8975_SECONDARY 00134 #endif /* #if defined AK8963_SECONDARY */ 00135 #elif defined MPU9250 /* #if defined MPU9150 */ 00136 #ifndef MPU6500 00137 #define MPU6500 00138 #endif /* #ifndef MPU6500 */ 00139 #if defined AK8975_SECONDARY 00140 #error "MPU9250 and AK8975_SECONDARY cannot both be defined." 00141 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ 00142 #define AK8963_SECONDARY 00143 #endif /* #if defined AK8975_SECONDARY */ 00144 #endif /* #if defined MPU9150 */ 00145 00146 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY 00147 #define AK89xx_SECONDARY 00148 #else 00149 /* #warning "No compass = less profit for Invensense. Lame." */ 00150 #endif 00151 00152 static int set_int_enable(unsigned char enable); 00153 00154 /* Hardware registers needed by driver. */ 00155 struct gyro_reg_s { 00156 unsigned char who_am_i; 00157 unsigned char rate_div; 00158 unsigned char lpf; 00159 unsigned char prod_id; 00160 unsigned char user_ctrl; 00161 unsigned char fifo_en; 00162 unsigned char gyro_cfg; 00163 unsigned char accel_cfg; 00164 unsigned char accel_cfg2; 00165 unsigned char lp_accel_odr; 00166 unsigned char motion_thr; 00167 unsigned char motion_dur; 00168 unsigned char fifo_count_h; 00169 unsigned char fifo_r_w; 00170 unsigned char raw_gyro; 00171 unsigned char raw_accel; 00172 unsigned char temp; 00173 unsigned char int_enable; 00174 unsigned char dmp_int_status; 00175 unsigned char int_status; 00176 unsigned char accel_intel; 00177 unsigned char pwr_mgmt_1; 00178 unsigned char pwr_mgmt_2; 00179 unsigned char int_pin_cfg; 00180 unsigned char mem_r_w; 00181 unsigned char accel_offs; 00182 unsigned char i2c_mst; 00183 unsigned char bank_sel; 00184 unsigned char mem_start_addr; 00185 unsigned char prgm_start_h; 00186 #if defined AK89xx_SECONDARY 00187 unsigned char s0_addr; 00188 unsigned char s0_reg; 00189 unsigned char s0_ctrl; 00190 unsigned char s1_addr; 00191 unsigned char s1_reg; 00192 unsigned char s1_ctrl; 00193 unsigned char s4_ctrl; 00194 unsigned char s0_do; 00195 unsigned char s1_do; 00196 unsigned char i2c_delay_ctrl; 00197 unsigned char raw_compass; 00198 /* The I2C_MST_VDDIO bit is in this register. */ 00199 unsigned char yg_offs_tc; 00200 #endif 00201 }; 00202 00203 /* Information specific to a particular device. */ 00204 struct hw_s { 00205 unsigned char addr; 00206 unsigned short max_fifo; 00207 unsigned char num_reg; 00208 unsigned short temp_sens; 00209 short temp_offset; 00210 unsigned short bank_size; 00211 #if defined AK89xx_SECONDARY 00212 unsigned short compass_fsr; 00213 #endif 00214 }; 00215 00216 /* When entering motion interrupt mode, the driver keeps track of the 00217 * previous state so that it can be restored at a later time. 00218 * TODO: This is tacky. Fix it. 00219 */ 00220 struct motion_int_cache_s { 00221 unsigned short gyro_fsr; 00222 unsigned char accel_fsr; 00223 unsigned short lpf; 00224 unsigned short sample_rate; 00225 unsigned char sensors_on; 00226 unsigned char fifo_sensors; 00227 unsigned char dmp_on; 00228 }; 00229 00230 /* Cached chip configuration data. 00231 * TODO: A lot of these can be handled with a bitmask. 00232 */ 00233 struct chip_cfg_s { 00234 /* Matches gyro_cfg >> 3 & 0x03 */ 00235 unsigned char gyro_fsr; 00236 /* Matches accel_cfg >> 3 & 0x03 */ 00237 unsigned char accel_fsr; 00238 /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ 00239 unsigned char sensors; 00240 /* Matches config register. */ 00241 unsigned char lpf; 00242 unsigned char clk_src; 00243 /* Sample rate, NOT rate divider. */ 00244 unsigned short sample_rate; 00245 /* Matches fifo_en register. */ 00246 unsigned char fifo_enable; 00247 /* Matches int enable register. */ 00248 unsigned char int_enable; 00249 /* 1 if devices on auxiliary I2C bus appear on the primary. */ 00250 unsigned char bypass_mode; 00251 /* 1 if half-sensitivity. 00252 * NOTE: This doesn't belong here, but everything else in hw_s is const, 00253 * and this allows us to save some precious RAM. 00254 */ 00255 unsigned char accel_half; 00256 /* 1 if device in low-power accel-only mode. */ 00257 unsigned char lp_accel_mode; 00258 /* 1 if interrupts are only triggered on motion events. */ 00259 unsigned char int_motion_only; 00260 struct motion_int_cache_s cache; 00261 /* 1 for active low interrupts. */ 00262 unsigned char active_low_int; 00263 /* 1 for latched interrupts. */ 00264 unsigned char latched_int; 00265 /* 1 if DMP is enabled. */ 00266 unsigned char dmp_on; 00267 /* Ensures that DMP will only be loaded once. */ 00268 unsigned char dmp_loaded; 00269 /* Sampling rate used when DMP is enabled. */ 00270 unsigned short dmp_sample_rate; 00271 #ifdef AK89xx_SECONDARY 00272 /* Compass sample rate. */ 00273 unsigned short compass_sample_rate; 00274 unsigned char compass_addr; 00275 short mag_sens_adj[3]; 00276 #endif 00277 }; 00278 00279 /* Information for self-test. */ 00280 struct test_s { 00281 unsigned long gyro_sens; 00282 unsigned long accel_sens; 00283 unsigned char reg_rate_div; 00284 unsigned char reg_lpf; 00285 unsigned char reg_gyro_fsr; 00286 unsigned char reg_accel_fsr; 00287 unsigned short wait_ms; 00288 unsigned char packet_thresh; 00289 float min_dps; 00290 float max_dps; 00291 float max_gyro_var; 00292 float min_g; 00293 float max_g; 00294 float max_accel_var; 00295 #ifdef MPU6500 00296 float max_g_offset; 00297 unsigned short sample_wait_ms; 00298 #endif 00299 }; 00300 00301 /* Gyro driver state variables. */ 00302 struct gyro_state_s { 00303 const struct gyro_reg_s *reg; 00304 const struct hw_s *hw; 00305 struct chip_cfg_s chip_cfg; 00306 const struct test_s *test; 00307 }; 00308 00309 /* Filter configurations. */ 00310 enum lpf_e { 00311 INV_FILTER_256HZ_NOLPF2 = 0, 00312 INV_FILTER_188HZ, 00313 INV_FILTER_98HZ, 00314 INV_FILTER_42HZ, 00315 INV_FILTER_20HZ, 00316 INV_FILTER_10HZ, 00317 INV_FILTER_5HZ, 00318 INV_FILTER_2100HZ_NOLPF, 00319 NUM_FILTER 00320 }; 00321 00322 /* Full scale ranges. */ 00323 enum gyro_fsr_e { 00324 INV_FSR_250DPS = 0, 00325 INV_FSR_500DPS, 00326 INV_FSR_1000DPS, 00327 INV_FSR_2000DPS, 00328 NUM_GYRO_FSR 00329 }; 00330 00331 /* Full scale ranges. */ 00332 enum accel_fsr_e { 00333 INV_FSR_2G = 0, 00334 INV_FSR_4G, 00335 INV_FSR_8G, 00336 INV_FSR_16G, 00337 NUM_ACCEL_FSR 00338 }; 00339 00340 /* Clock sources. */ 00341 enum clock_sel_e { 00342 INV_CLK_INTERNAL = 0, 00343 INV_CLK_PLL, 00344 NUM_CLK 00345 }; 00346 00347 /* Low-power accel wakeup rates. */ 00348 enum lp_accel_rate_e { 00349 #if defined MPU6050 00350 INV_LPA_1_25HZ, 00351 INV_LPA_5HZ, 00352 INV_LPA_20HZ, 00353 INV_LPA_40HZ 00354 #elif defined MPU6500 00355 INV_LPA_0_3125HZ, 00356 INV_LPA_0_625HZ, 00357 INV_LPA_1_25HZ, 00358 INV_LPA_2_5HZ, 00359 INV_LPA_5HZ, 00360 INV_LPA_10HZ, 00361 INV_LPA_20HZ, 00362 INV_LPA_40HZ, 00363 INV_LPA_80HZ, 00364 INV_LPA_160HZ, 00365 INV_LPA_320HZ, 00366 INV_LPA_640HZ 00367 #endif 00368 }; 00369 00370 #define BIT_I2C_MST_VDDIO (0x80) 00371 #define BIT_FIFO_EN (0x40) 00372 #define BIT_DMP_EN (0x80) 00373 #define BIT_FIFO_RST (0x04) 00374 #define BIT_DMP_RST (0x08) 00375 #define BIT_FIFO_OVERFLOW (0x10) 00376 #define BIT_DATA_RDY_EN (0x01) 00377 #define BIT_DMP_INT_EN (0x02) 00378 #define BIT_MOT_INT_EN (0x40) 00379 #define BITS_FSR (0x18) 00380 #define BITS_LPF (0x07) 00381 #define BITS_HPF (0x07) 00382 #define BITS_CLK (0x07) 00383 #define BIT_FIFO_SIZE_1024 (0x40) 00384 #define BIT_FIFO_SIZE_2048 (0x80) 00385 #define BIT_FIFO_SIZE_4096 (0xC0) 00386 #define BIT_RESET (0x80) 00387 #define BIT_SLEEP (0x40) 00388 #define BIT_S0_DELAY_EN (0x01) 00389 #define BIT_S2_DELAY_EN (0x04) 00390 #define BITS_SLAVE_LENGTH (0x0F) 00391 #define BIT_SLAVE_BYTE_SW (0x40) 00392 #define BIT_SLAVE_GROUP (0x10) 00393 #define BIT_SLAVE_EN (0x80) 00394 #define BIT_I2C_READ (0x80) 00395 #define BITS_I2C_MASTER_DLY (0x1F) 00396 #define BIT_AUX_IF_EN (0x20) 00397 #define BIT_ACTL (0x80) 00398 #define BIT_LATCH_EN (0x20) 00399 #define BIT_ANY_RD_CLR (0x10) 00400 #define BIT_BYPASS_EN (0x02) 00401 #define BITS_WOM_EN (0xC0) 00402 #define BIT_LPA_CYCLE (0x20) 00403 #define BIT_STBY_XA (0x20) 00404 #define BIT_STBY_YA (0x10) 00405 #define BIT_STBY_ZA (0x08) 00406 #define BIT_STBY_XG (0x04) 00407 #define BIT_STBY_YG (0x02) 00408 #define BIT_STBY_ZG (0x01) 00409 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) 00410 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) 00411 00412 #if defined AK8975_SECONDARY 00413 #define SUPPORTS_AK89xx_HIGH_SENS (0x00) 00414 #define AK89xx_FSR (9830) 00415 #elif defined AK8963_SECONDARY 00416 #define SUPPORTS_AK89xx_HIGH_SENS (0x10) 00417 #define AK89xx_FSR (4915) 00418 #endif 00419 00420 #ifdef AK89xx_SECONDARY 00421 #define AKM_REG_WHOAMI (0x00) 00422 00423 #define AKM_REG_ST1 (0x02) 00424 #define AKM_REG_HXL (0x03) 00425 #define AKM_REG_ST2 (0x09) 00426 00427 #define AKM_REG_CNTL (0x0A) 00428 #define AKM_REG_ASTC (0x0C) 00429 #define AKM_REG_ASAX (0x10) 00430 #define AKM_REG_ASAY (0x11) 00431 #define AKM_REG_ASAZ (0x12) 00432 00433 #define AKM_DATA_READY (0x01) 00434 #define AKM_DATA_OVERRUN (0x02) 00435 #define AKM_OVERFLOW (0x80) 00436 #define AKM_DATA_ERROR (0x40) 00437 00438 #define AKM_BIT_SELF_TEST (0x40) 00439 00440 #define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) 00441 #define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) 00442 #define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) 00443 #define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) 00444 00445 #define AKM_WHOAMI (0x48) 00446 #endif 00447 00448 #if defined MPU6050 00449 const struct gyro_reg_s reg = { 00450 .who_am_i = 0x75, 00451 .rate_div = 0x19, 00452 .lpf = 0x1A, 00453 .prod_id = 0x0C, 00454 .user_ctrl = 0x6A, 00455 .fifo_en = 0x23, 00456 .gyro_cfg = 0x1B, 00457 .accel_cfg = 0x1C, 00458 .motion_thr = 0x1F, 00459 .motion_dur = 0x20, 00460 .fifo_count_h = 0x72, 00461 .fifo_r_w = 0x74, 00462 .raw_gyro = 0x43, 00463 .raw_accel = 0x3B, 00464 .temp = 0x41, 00465 .int_enable = 0x38, 00466 .dmp_int_status = 0x39, 00467 .int_status = 0x3A, 00468 .pwr_mgmt_1 = 0x6B, 00469 .pwr_mgmt_2 = 0x6C, 00470 .int_pin_cfg = 0x37, 00471 .mem_r_w = 0x6F, 00472 .accel_offs = 0x06, 00473 .i2c_mst = 0x24, 00474 .bank_sel = 0x6D, 00475 .mem_start_addr = 0x6E, 00476 .prgm_start_h = 0x70 00477 #ifdef AK89xx_SECONDARY 00478 ,.raw_compass = 0x49, 00479 .yg_offs_tc = 0x01, 00480 .s0_addr = 0x25, 00481 .s0_reg = 0x26, 00482 .s0_ctrl = 0x27, 00483 .s1_addr = 0x28, 00484 .s1_reg = 0x29, 00485 .s1_ctrl = 0x2A, 00486 .s4_ctrl = 0x34, 00487 .s0_do = 0x63, 00488 .s1_do = 0x64, 00489 .i2c_delay_ctrl = 0x67 00490 #endif 00491 }; 00492 const struct hw_s hw = { 00493 .addr = 0x69, 00494 .max_fifo = 1024, 00495 .num_reg = 118, 00496 .temp_sens = 340, 00497 .temp_offset = -521, 00498 .bank_size = 256 00499 #if defined AK89xx_SECONDARY 00500 ,.compass_fsr = AK89xx_FSR 00501 #endif 00502 }; 00503 00504 const struct test_s test = { 00505 .gyro_sens = 32768/250, 00506 .accel_sens = 32768/16, 00507 .reg_rate_div = 0, /* 1kHz. */ 00508 .reg_lpf = 1, /* 188Hz. */ 00509 .reg_gyro_fsr = 0, /* 250dps. */ 00510 .reg_accel_fsr = 0x18, /* 16g. */ 00511 .wait_ms = 50, 00512 .packet_thresh = 5, /* 5% */ 00513 .min_dps = 10.f, 00514 .max_dps = 105.f, 00515 .max_gyro_var = 0.14f, 00516 .min_g = 0.3f, 00517 .max_g = 0.95f, 00518 .max_accel_var = 0.14f 00519 }; 00520 00521 static struct gyro_state_s st = { 00522 .reg = ®, 00523 .hw = &hw, 00524 .test = &test 00525 }; 00526 #elif defined MPU6500 00527 const struct gyro_reg_s reg = { 00528 .who_am_i = 0x75, 00529 .rate_div = 0x19, 00530 .lpf = 0x1A, 00531 .prod_id = 0x0C, 00532 .user_ctrl = 0x6A, 00533 .fifo_en = 0x23, 00534 .gyro_cfg = 0x1B, 00535 .accel_cfg = 0x1C, 00536 .accel_cfg2 = 0x1D, 00537 .lp_accel_odr = 0x1E, 00538 .motion_thr = 0x1F, 00539 .motion_dur = 0x20, 00540 .fifo_count_h = 0x72, 00541 .fifo_r_w = 0x74, 00542 .raw_gyro = 0x43, 00543 .raw_accel = 0x3B, 00544 .temp = 0x41, 00545 .int_enable = 0x38, 00546 .dmp_int_status = 0x39, 00547 .int_status = 0x3A, 00548 .accel_intel = 0x69, 00549 .pwr_mgmt_1 = 0x6B, 00550 .pwr_mgmt_2 = 0x6C, 00551 .int_pin_cfg = 0x37, 00552 .mem_r_w = 0x6F, 00553 .accel_offs = 0x77, 00554 .i2c_mst = 0x24, 00555 .bank_sel = 0x6D, 00556 .mem_start_addr = 0x6E, 00557 .prgm_start_h = 0x70 00558 #ifdef AK89xx_SECONDARY 00559 ,.raw_compass = 0x49, 00560 .s0_addr = 0x25, 00561 .s0_reg = 0x26, 00562 .s0_ctrl = 0x27, 00563 .s1_addr = 0x28, 00564 .s1_reg = 0x29, 00565 .s1_ctrl = 0x2A, 00566 .s4_ctrl = 0x34, 00567 .s0_do = 0x63, 00568 .s1_do = 0x64, 00569 .i2c_delay_ctrl = 0x67 00570 #endif 00571 }; 00572 const struct hw_s hw = { 00573 .addr = 0x68, 00574 .max_fifo = 1024, 00575 .num_reg = 128, 00576 .temp_sens = 321, 00577 .temp_offset = 0, 00578 .bank_size = 256 00579 #if defined AK89xx_SECONDARY 00580 ,.compass_fsr = AK89xx_FSR 00581 #endif 00582 }; 00583 00584 const struct test_s test = { 00585 .gyro_sens = 32768/250, 00586 .accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G 00587 .reg_rate_div = 0, /* 1kHz. */ 00588 .reg_lpf = 2, /* 92Hz low pass filter*/ 00589 .reg_gyro_fsr = 0, /* 250dps. */ 00590 .reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */ 00591 .wait_ms = 200, //200ms stabilization time 00592 .packet_thresh = 200, /* 200 samples */ 00593 .min_dps = 20.f, //20 dps for Gyro Criteria C 00594 .max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B 00595 .max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A 00596 .min_g = .225f, //Accel must exceed Min 225 mg for Criteria B 00597 .max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B 00598 .max_accel_var = .5f, //Accel must be within 50% variation for Criteria A 00599 .max_g_offset = .5f, //500 mg for Accel Criteria C 00600 .sample_wait_ms = 10 //10ms sample time wait 00601 }; 00602 00603 static struct gyro_state_s st = { 00604 .reg = ®, 00605 .hw = &hw, 00606 .test = &test 00607 }; 00608 #endif 00609 00610 #define MAX_PACKET_LENGTH (12) 00611 #ifdef MPU6500 00612 #define HWST_MAX_PACKET_LENGTH (512) 00613 #endif 00614 00615 #ifdef AK89xx_SECONDARY 00616 static int setup_compass(void); 00617 #define MAX_COMPASS_SAMPLE_RATE (100) 00618 #endif 00619 00620 /** 00621 * @brief Enable/disable data ready interrupt. 00622 * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready 00623 * interrupt is used. 00624 * @param[in] enable 1 to enable interrupt. 00625 * @return 0 if successful. 00626 */ 00627 static int set_int_enable(unsigned char enable) 00628 { 00629 unsigned char tmp; 00630 00631 if (st.chip_cfg.dmp_on) { 00632 if (enable) 00633 tmp = BIT_DMP_INT_EN; 00634 else 00635 tmp = 0x00; 00636 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 00637 return -1; 00638 st.chip_cfg.int_enable = tmp; 00639 } else { 00640 if (!st.chip_cfg.sensors) 00641 return -1; 00642 if (enable && st.chip_cfg.int_enable) 00643 return 0; 00644 if (enable) 00645 tmp = BIT_DATA_RDY_EN; 00646 else 00647 tmp = 0x00; 00648 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 00649 return -1; 00650 st.chip_cfg.int_enable = tmp; 00651 } 00652 return 0; 00653 } 00654 00655 /** 00656 * @brief Register dump for testing. 00657 * @return 0 if successful. 00658 */ 00659 int mpu_reg_dump(void) 00660 { 00661 unsigned char ii; 00662 unsigned char data; 00663 00664 for (ii = 0; ii < st.hw->num_reg; ii++) { 00665 if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) 00666 continue; 00667 if (mpu_hal_read(st.hw->addr, ii, 1, &data)) 00668 return -1; 00669 log_i("%#5x: %#5x\r\n", ii, data); 00670 } 00671 return 0; 00672 } 00673 00674 /** 00675 * @brief Read from a single register. 00676 * NOTE: The memory and FIFO read/write registers cannot be accessed. 00677 * @param[in] reg Register address. 00678 * @param[out] data Register data. 00679 * @return 0 if successful. 00680 */ 00681 int mpu_read_reg(unsigned char reg, unsigned char *data) 00682 { 00683 if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) 00684 return -1; 00685 if (reg >= st.hw->num_reg) 00686 return -1; 00687 return mpu_hal_read(st.hw->addr, reg, 1, data); 00688 } 00689 00690 /** 00691 * @brief Initialize hardware. 00692 * Initial configuration:\n 00693 * Gyro FSR: +/- 2000DPS\n 00694 * Accel FSR +/- 2G\n 00695 * DLPF: 42Hz\n 00696 * FIFO rate: 50Hz\n 00697 * Clock source: Gyro PLL\n 00698 * FIFO: Disabled.\n 00699 * Data ready interrupt: Disabled, active low, unlatched. 00700 * @param[in] int_param Platform-specific parameters to interrupt API. 00701 * @return 0 if successful. 00702 */ 00703 int mpu_init(struct int_param_s *int_param) 00704 { 00705 unsigned char data[6]; 00706 00707 /* Reset device. */ 00708 data[0] = BIT_RESET; 00709 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 00710 return -1; 00711 delay_ms(100); 00712 00713 /* Wake up chip. */ 00714 data[0] = 0x00; 00715 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 00716 return -1; 00717 00718 st.chip_cfg.accel_half = 0; 00719 00720 #ifdef MPU6500 00721 /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the 00722 * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. 00723 */ 00724 data[0] = BIT_FIFO_SIZE_1024 | 0x8; 00725 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) 00726 return -1; 00727 #endif 00728 00729 /* Set to invalid values to ensure no I2C writes are skipped. */ 00730 st.chip_cfg.sensors = 0xFF; 00731 st.chip_cfg.gyro_fsr = 0xFF; 00732 st.chip_cfg.accel_fsr = 0xFF; 00733 st.chip_cfg.lpf = 0xFF; 00734 st.chip_cfg.sample_rate = 0xFFFF; 00735 st.chip_cfg.fifo_enable = 0xFF; 00736 st.chip_cfg.bypass_mode = 0xFF; 00737 #ifdef AK89xx_SECONDARY 00738 st.chip_cfg.compass_sample_rate = 0xFFFF; 00739 #endif 00740 /* mpu_set_sensors always preserves this setting. */ 00741 st.chip_cfg.clk_src = INV_CLK_PLL; 00742 /* Handled in next call to mpu_set_bypass. */ 00743 st.chip_cfg.active_low_int = 1; 00744 st.chip_cfg.latched_int = 0; 00745 st.chip_cfg.int_motion_only = 0; 00746 st.chip_cfg.lp_accel_mode = 0; 00747 memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); 00748 st.chip_cfg.dmp_on = 0; 00749 st.chip_cfg.dmp_loaded = 0; 00750 st.chip_cfg.dmp_sample_rate = 0; 00751 00752 if (mpu_set_gyro_fsr(2000)) 00753 return -1; 00754 if (mpu_set_accel_fsr(2)) 00755 return -1; 00756 if (mpu_set_lpf(42)) 00757 return -1; 00758 if (mpu_set_sample_rate(50)) 00759 return -1; 00760 if (mpu_configure_fifo(0)) 00761 return -1; 00762 00763 if (int_param) 00764 reg_int_cb(int_param); 00765 00766 #ifdef AK89xx_SECONDARY 00767 setup_compass(); 00768 if (mpu_set_compass_sample_rate(10)) 00769 return -1; 00770 #else 00771 /* Already disabled by setup_compass. */ 00772 if (mpu_set_bypass(0)) 00773 return -1; 00774 #endif 00775 00776 mpu_set_sensors(0); 00777 return 0; 00778 } 00779 00780 /** 00781 * @brief Enter low-power accel-only mode. 00782 * In low-power accel mode, the chip goes to sleep and only wakes up to sample 00783 * the accelerometer at one of the following frequencies: 00784 * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz 00785 * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz 00786 * \n If the requested rate is not one listed above, the device will be set to 00787 * the next highest rate. Requesting a rate above the maximum supported 00788 * frequency will result in an error. 00789 * \n To select a fractional wake-up frequency, round down the value passed to 00790 * @e rate. 00791 * @param[in] rate Minimum sampling rate, or zero to disable LP 00792 * accel mode. 00793 * @return 0 if successful. 00794 */ 00795 int mpu_lp_accel_mode(unsigned char rate) 00796 { 00797 unsigned char tmp[2]; 00798 00799 if (rate > 40) 00800 return -1; 00801 00802 if (!rate) { 00803 mpu_set_int_latched(0); 00804 tmp[0] = 0; 00805 tmp[1] = BIT_STBY_XYZG; 00806 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 00807 return -1; 00808 st.chip_cfg.lp_accel_mode = 0; 00809 return 0; 00810 } 00811 /* For LP accel, we automatically configure the hardware to produce latched 00812 * interrupts. In LP accel mode, the hardware cycles into sleep mode before 00813 * it gets a chance to deassert the interrupt pin; therefore, we shift this 00814 * responsibility over to the MCU. 00815 * 00816 * Any register read will clear the interrupt. 00817 */ 00818 mpu_set_int_latched(1); 00819 #if defined MPU6050 00820 tmp[0] = BIT_LPA_CYCLE; 00821 if (rate == 1) { 00822 tmp[1] = INV_LPA_1_25HZ; 00823 mpu_set_lpf(5); 00824 } else if (rate <= 5) { 00825 tmp[1] = INV_LPA_5HZ; 00826 mpu_set_lpf(5); 00827 } else if (rate <= 20) { 00828 tmp[1] = INV_LPA_20HZ; 00829 mpu_set_lpf(10); 00830 } else { 00831 tmp[1] = INV_LPA_40HZ; 00832 mpu_set_lpf(20); 00833 } 00834 tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; 00835 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 00836 return -1; 00837 #elif defined MPU6500 00838 /* Set wake frequency. */ 00839 if (rate == 1) 00840 tmp[0] = INV_LPA_1_25HZ; 00841 else if (rate == 2) 00842 tmp[0] = INV_LPA_2_5HZ; 00843 else if (rate <= 5) 00844 tmp[0] = INV_LPA_5HZ; 00845 else if (rate <= 10) 00846 tmp[0] = INV_LPA_10HZ; 00847 else if (rate <= 20) 00848 tmp[0] = INV_LPA_20HZ; 00849 else if (rate <= 40) 00850 tmp[0] = INV_LPA_40HZ; 00851 else if (rate <= 80) 00852 tmp[0] = INV_LPA_80HZ; 00853 else if (rate <= 160) 00854 tmp[0] = INV_LPA_160HZ; 00855 else if (rate <= 320) 00856 tmp[0] = INV_LPA_320HZ; 00857 else 00858 tmp[0] = INV_LPA_640HZ; 00859 if (mpu_hal_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) 00860 return -1; 00861 tmp[0] = BIT_LPA_CYCLE; 00862 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) 00863 return -1; 00864 #endif 00865 st.chip_cfg.sensors = INV_XYZ_ACCEL; 00866 st.chip_cfg.clk_src = 0; 00867 st.chip_cfg.lp_accel_mode = 1; 00868 mpu_configure_fifo(0); 00869 00870 return 0; 00871 } 00872 00873 /** 00874 * @brief Read raw gyro data directly from the registers. 00875 * @param[out] data Raw data in hardware units. 00876 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 00877 * @return 0 if successful. 00878 */ 00879 int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 00880 { 00881 unsigned char tmp[6]; 00882 00883 if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) 00884 return -1; 00885 00886 if (mpu_hal_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) 00887 return -1; 00888 data[0] = (tmp[0] << 8) | tmp[1]; 00889 data[1] = (tmp[2] << 8) | tmp[3]; 00890 data[2] = (tmp[4] << 8) | tmp[5]; 00891 if (timestamp) 00892 get_ms(timestamp); 00893 return 0; 00894 } 00895 00896 /** 00897 * @brief Read raw accel data directly from the registers. 00898 * @param[out] data Raw data in hardware units. 00899 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 00900 * @return 0 if successful. 00901 */ 00902 int mpu_get_accel_reg(short *data, unsigned long *timestamp) 00903 { 00904 unsigned char tmp[6]; 00905 00906 if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) 00907 return -1; 00908 00909 if (mpu_hal_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) 00910 return -1; 00911 data[0] = (tmp[0] << 8) | tmp[1]; 00912 data[1] = (tmp[2] << 8) | tmp[3]; 00913 data[2] = (tmp[4] << 8) | tmp[5]; 00914 if (timestamp) 00915 get_ms(timestamp); 00916 return 0; 00917 } 00918 00919 /** 00920 * @brief Read temperature data directly from the registers. 00921 * @param[out] data Data in q16 format. 00922 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 00923 * @return 0 if successful. 00924 */ 00925 int mpu_get_temperature(long *data, unsigned long *timestamp) 00926 { 00927 unsigned char tmp[2]; 00928 short raw; 00929 00930 if (!(st.chip_cfg.sensors)) 00931 return -1; 00932 00933 if (mpu_hal_read(st.hw->addr, st.reg->temp, 2, tmp)) 00934 return -1; 00935 raw = (tmp[0] << 8) | tmp[1]; 00936 if (timestamp) 00937 get_ms(timestamp); 00938 00939 data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); 00940 return 0; 00941 } 00942 00943 /** 00944 * @brief Read biases to the accel bias 6500 registers. 00945 * This function reads from the MPU6500 accel offset cancellations registers. 00946 * The format are G in +-8G format. The register is initialized with OTP 00947 * factory trim values. 00948 * @param[in] accel_bias returned structure with the accel bias 00949 * @return 0 if successful. 00950 */ 00951 int mpu_read_6500_accel_bias(long *accel_bias) { 00952 unsigned char data[6]; 00953 if (mpu_hal_read(st.hw->addr, 0x77, 2, &data[0])) 00954 return -1; 00955 if (mpu_hal_read(st.hw->addr, 0x7A, 2, &data[2])) 00956 return -1; 00957 if (mpu_hal_read(st.hw->addr, 0x7D, 2, &data[4])) 00958 return -1; 00959 accel_bias[0] = ((long)data[0]<<8) | data[1]; 00960 accel_bias[1] = ((long)data[2]<<8) | data[3]; 00961 accel_bias[2] = ((long)data[4]<<8) | data[5]; 00962 return 0; 00963 } 00964 00965 /** 00966 * @brief Read biases to the accel bias 6050 registers. 00967 * This function reads from the MPU6050 accel offset cancellations registers. 00968 * The format are G in +-8G format. The register is initialized with OTP 00969 * factory trim values. 00970 * @param[in] accel_bias returned structure with the accel bias 00971 * @return 0 if successful. 00972 */ 00973 int mpu_read_6050_accel_bias(long *accel_bias) { 00974 unsigned char data[6]; 00975 if (mpu_hal_read(st.hw->addr, 0x06, 2, &data[0])) 00976 return -1; 00977 if (mpu_hal_read(st.hw->addr, 0x08, 2, &data[2])) 00978 return -1; 00979 if (mpu_hal_read(st.hw->addr, 0x0A, 2, &data[4])) 00980 return -1; 00981 accel_bias[0] = ((long)data[0]<<8) | data[1]; 00982 accel_bias[1] = ((long)data[2]<<8) | data[3]; 00983 accel_bias[2] = ((long)data[4]<<8) | data[5]; 00984 return 0; 00985 } 00986 00987 int mpu_read_6500_gyro_bias(long *gyro_bias) { 00988 unsigned char data[6]; 00989 if (mpu_hal_read(st.hw->addr, 0x13, 2, &data[0])) 00990 return -1; 00991 if (mpu_hal_read(st.hw->addr, 0x15, 2, &data[2])) 00992 return -1; 00993 if (mpu_hal_read(st.hw->addr, 0x17, 2, &data[4])) 00994 return -1; 00995 gyro_bias[0] = ((long)data[0]<<8) | data[1]; 00996 gyro_bias[1] = ((long)data[2]<<8) | data[3]; 00997 gyro_bias[2] = ((long)data[4]<<8) | data[5]; 00998 return 0; 00999 } 01000 01001 /** 01002 * @brief Push biases to the gyro bias 6500/6050 registers. 01003 * This function expects biases relative to the current sensor output, and 01004 * these biases will be added to the factory-supplied values. Bias inputs are LSB 01005 * in +-1000dps format. 01006 * @param[in] gyro_bias New biases. 01007 * @return 0 if successful. 01008 */ 01009 int mpu_set_gyro_bias_reg(long *gyro_bias) 01010 { 01011 unsigned char data[6] = {0, 0, 0, 0, 0, 0}; 01012 int i=0; 01013 for(i=0;i<3;i++) { 01014 gyro_bias[i]= (-gyro_bias[i]); 01015 } 01016 data[0] = (gyro_bias[0] >> 8) & 0xff; 01017 data[1] = (gyro_bias[0]) & 0xff; 01018 data[2] = (gyro_bias[1] >> 8) & 0xff; 01019 data[3] = (gyro_bias[1]) & 0xff; 01020 data[4] = (gyro_bias[2] >> 8) & 0xff; 01021 data[5] = (gyro_bias[2]) & 0xff; 01022 if (mpu_hal_write(st.hw->addr, 0x13, 2, &data[0])) 01023 return -1; 01024 if (mpu_hal_write(st.hw->addr, 0x15, 2, &data[2])) 01025 return -1; 01026 if (mpu_hal_write(st.hw->addr, 0x17, 2, &data[4])) 01027 return -1; 01028 return 0; 01029 } 01030 01031 /** 01032 * @brief Push biases to the accel bias 6050 registers. 01033 * This function expects biases relative to the current sensor output, and 01034 * these biases will be added to the factory-supplied values. Bias inputs are LSB 01035 * in +-8G format. 01036 * @param[in] accel_bias New biases. 01037 * @return 0 if successful. 01038 */ 01039 int mpu_set_accel_bias_6050_reg(const long *accel_bias) 01040 { 01041 unsigned char data[6] = {0, 0, 0, 0, 0, 0}; 01042 long accel_reg_bias[3] = {0, 0, 0}; 01043 long mask = 0x0001; 01044 unsigned char mask_bit[3] = {0, 0, 0}; 01045 unsigned char i = 0; 01046 if(mpu_read_6050_accel_bias(accel_reg_bias)) 01047 return -1; 01048 01049 //bit 0 of the 2 byte bias is for temp comp 01050 //calculations need to compensate for this and not change it 01051 for(i=0; i<3; i++) { 01052 if(accel_reg_bias[i]&mask) 01053 mask_bit[i] = 0x01; 01054 } 01055 01056 accel_reg_bias[0] -= accel_bias[0]; 01057 accel_reg_bias[1] -= accel_bias[1]; 01058 accel_reg_bias[2] -= accel_bias[2]; 01059 01060 data[0] = (accel_reg_bias[0] >> 8) & 0xff; 01061 data[1] = (accel_reg_bias[0]) & 0xff; 01062 data[1] = data[1]|mask_bit[0]; 01063 data[2] = (accel_reg_bias[1] >> 8) & 0xff; 01064 data[3] = (accel_reg_bias[1]) & 0xff; 01065 data[3] = data[3]|mask_bit[1]; 01066 data[4] = (accel_reg_bias[2] >> 8) & 0xff; 01067 data[5] = (accel_reg_bias[2]) & 0xff; 01068 data[5] = data[5]|mask_bit[2]; 01069 01070 if (mpu_hal_write(st.hw->addr, 0x06, 2, &data[0])) 01071 return -1; 01072 if (mpu_hal_write(st.hw->addr, 0x08, 2, &data[2])) 01073 return -1; 01074 if (mpu_hal_write(st.hw->addr, 0x0A, 2, &data[4])) 01075 return -1; 01076 01077 return 0; 01078 } 01079 01080 01081 /** 01082 * @brief Push biases to the accel bias 6500 registers. 01083 * This function expects biases relative to the current sensor output, and 01084 * these biases will be added to the factory-supplied values. Bias inputs are LSB 01085 * in +-8G format. 01086 * @param[in] accel_bias New biases. 01087 * @return 0 if successful. 01088 */ 01089 int mpu_set_accel_bias_6500_reg(const long *accel_bias) 01090 { 01091 unsigned char data[6] = {0, 0, 0, 0, 0, 0}; 01092 long accel_reg_bias[3] = {0, 0, 0}; 01093 long mask = 0x0001; 01094 unsigned char mask_bit[3] = {0, 0, 0}; 01095 unsigned char i = 0; 01096 01097 if(mpu_read_6500_accel_bias(accel_reg_bias)) 01098 return -1; 01099 01100 //bit 0 of the 2 byte bias is for temp comp 01101 //calculations need to compensate for this 01102 for(i=0; i<3; i++) { 01103 if(accel_reg_bias[i]&mask) 01104 mask_bit[i] = 0x01; 01105 } 01106 01107 accel_reg_bias[0] -= accel_bias[0]; 01108 accel_reg_bias[1] -= accel_bias[1]; 01109 accel_reg_bias[2] -= accel_bias[2]; 01110 01111 data[0] = (accel_reg_bias[0] >> 8) & 0xff; 01112 data[1] = (accel_reg_bias[0]) & 0xff; 01113 data[1] = data[1]|mask_bit[0]; 01114 data[2] = (accel_reg_bias[1] >> 8) & 0xff; 01115 data[3] = (accel_reg_bias[1]) & 0xff; 01116 data[3] = data[3]|mask_bit[1]; 01117 data[4] = (accel_reg_bias[2] >> 8) & 0xff; 01118 data[5] = (accel_reg_bias[2]) & 0xff; 01119 data[5] = data[5]|mask_bit[2]; 01120 01121 if (mpu_hal_write(st.hw->addr, 0x77, 2, &data[0])) 01122 return -1; 01123 if (mpu_hal_write(st.hw->addr, 0x7A, 2, &data[2])) 01124 return -1; 01125 if (mpu_hal_write(st.hw->addr, 0x7D, 2, &data[4])) 01126 return -1; 01127 01128 return 0; 01129 } 01130 01131 /** 01132 * @brief Reset FIFO read/write pointers. 01133 * @return 0 if successful. 01134 */ 01135 int mpu_reset_fifo(void) 01136 { 01137 unsigned char data; 01138 01139 if (!(st.chip_cfg.sensors)) 01140 return -1; 01141 01142 data = 0; 01143 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01144 return -1; 01145 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01146 return -1; 01147 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01148 return -1; 01149 01150 if (st.chip_cfg.dmp_on) { 01151 data = BIT_FIFO_RST | BIT_DMP_RST; 01152 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01153 return -1; 01154 delay_ms(50); 01155 data = BIT_DMP_EN | BIT_FIFO_EN; 01156 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 01157 data |= BIT_AUX_IF_EN; 01158 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01159 return -1; 01160 if (st.chip_cfg.int_enable) 01161 data = BIT_DMP_INT_EN; 01162 else 01163 data = 0; 01164 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01165 return -1; 01166 data = 0; 01167 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01168 return -1; 01169 } else { 01170 data = BIT_FIFO_RST; 01171 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01172 return -1; 01173 if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 01174 data = BIT_FIFO_EN; 01175 else 01176 data = BIT_FIFO_EN | BIT_AUX_IF_EN; 01177 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01178 return -1; 01179 delay_ms(50); 01180 if (st.chip_cfg.int_enable) 01181 data = BIT_DATA_RDY_EN; 01182 else 01183 data = 0; 01184 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01185 return -1; 01186 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) 01187 return -1; 01188 } 01189 return 0; 01190 } 01191 01192 /** 01193 * @brief Get the gyro full-scale range. 01194 * @param[out] fsr Current full-scale range. 01195 * @return 0 if successful. 01196 */ 01197 int mpu_get_gyro_fsr(unsigned short *fsr) 01198 { 01199 switch (st.chip_cfg.gyro_fsr) { 01200 case INV_FSR_250DPS: 01201 fsr[0] = 250; 01202 break; 01203 case INV_FSR_500DPS: 01204 fsr[0] = 500; 01205 break; 01206 case INV_FSR_1000DPS: 01207 fsr[0] = 1000; 01208 break; 01209 case INV_FSR_2000DPS: 01210 fsr[0] = 2000; 01211 break; 01212 default: 01213 fsr[0] = 0; 01214 break; 01215 } 01216 return 0; 01217 } 01218 01219 /** 01220 * @brief Set the gyro full-scale range. 01221 * @param[in] fsr Desired full-scale range. 01222 * @return 0 if successful. 01223 */ 01224 int mpu_set_gyro_fsr(unsigned short fsr) 01225 { 01226 unsigned char data; 01227 01228 if (!(st.chip_cfg.sensors)) 01229 return -1; 01230 01231 switch (fsr) { 01232 case 250: 01233 data = INV_FSR_250DPS << 3; 01234 break; 01235 case 500: 01236 data = INV_FSR_500DPS << 3; 01237 break; 01238 case 1000: 01239 data = INV_FSR_1000DPS << 3; 01240 break; 01241 case 2000: 01242 data = INV_FSR_2000DPS << 3; 01243 break; 01244 default: 01245 return -1; 01246 } 01247 01248 if (st.chip_cfg.gyro_fsr == (data >> 3)) 01249 return 0; 01250 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) 01251 return -1; 01252 st.chip_cfg.gyro_fsr = data >> 3; 01253 return 0; 01254 } 01255 01256 /** 01257 * @brief Get the accel full-scale range. 01258 * @param[out] fsr Current full-scale range. 01259 * @return 0 if successful. 01260 */ 01261 int mpu_get_accel_fsr(unsigned char *fsr) 01262 { 01263 switch (st.chip_cfg.accel_fsr) { 01264 case INV_FSR_2G: 01265 fsr[0] = 2; 01266 break; 01267 case INV_FSR_4G: 01268 fsr[0] = 4; 01269 break; 01270 case INV_FSR_8G: 01271 fsr[0] = 8; 01272 break; 01273 case INV_FSR_16G: 01274 fsr[0] = 16; 01275 break; 01276 default: 01277 return -1; 01278 } 01279 if (st.chip_cfg.accel_half) 01280 fsr[0] <<= 1; 01281 return 0; 01282 } 01283 01284 /** 01285 * @brief Set the accel full-scale range. 01286 * @param[in] fsr Desired full-scale range. 01287 * @return 0 if successful. 01288 */ 01289 int mpu_set_accel_fsr(unsigned char fsr) 01290 { 01291 unsigned char data; 01292 01293 if (!(st.chip_cfg.sensors)) 01294 return -1; 01295 01296 switch (fsr) { 01297 case 2: 01298 data = INV_FSR_2G << 3; 01299 break; 01300 case 4: 01301 data = INV_FSR_4G << 3; 01302 break; 01303 case 8: 01304 data = INV_FSR_8G << 3; 01305 break; 01306 case 16: 01307 data = INV_FSR_16G << 3; 01308 break; 01309 default: 01310 return -1; 01311 } 01312 01313 if (st.chip_cfg.accel_fsr == (data >> 3)) 01314 return 0; 01315 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) 01316 return -1; 01317 st.chip_cfg.accel_fsr = data >> 3; 01318 return 0; 01319 } 01320 01321 /** 01322 * @brief Get the current DLPF setting. 01323 * @param[out] lpf Current LPF setting. 01324 * 0 if successful. 01325 */ 01326 int mpu_get_lpf(unsigned short *lpf) 01327 { 01328 switch (st.chip_cfg.lpf) { 01329 case INV_FILTER_188HZ: 01330 lpf[0] = 188; 01331 break; 01332 case INV_FILTER_98HZ: 01333 lpf[0] = 98; 01334 break; 01335 case INV_FILTER_42HZ: 01336 lpf[0] = 42; 01337 break; 01338 case INV_FILTER_20HZ: 01339 lpf[0] = 20; 01340 break; 01341 case INV_FILTER_10HZ: 01342 lpf[0] = 10; 01343 break; 01344 case INV_FILTER_5HZ: 01345 lpf[0] = 5; 01346 break; 01347 case INV_FILTER_256HZ_NOLPF2: 01348 case INV_FILTER_2100HZ_NOLPF: 01349 default: 01350 lpf[0] = 0; 01351 break; 01352 } 01353 return 0; 01354 } 01355 01356 /** 01357 * @brief Set digital low pass filter. 01358 * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. 01359 * @param[in] lpf Desired LPF setting. 01360 * @return 0 if successful. 01361 */ 01362 int mpu_set_lpf(unsigned short lpf) 01363 { 01364 unsigned char data; 01365 01366 if (!(st.chip_cfg.sensors)) 01367 return -1; 01368 01369 if (lpf >= 188) 01370 data = INV_FILTER_188HZ; 01371 else if (lpf >= 98) 01372 data = INV_FILTER_98HZ; 01373 else if (lpf >= 42) 01374 data = INV_FILTER_42HZ; 01375 else if (lpf >= 20) 01376 data = INV_FILTER_20HZ; 01377 else if (lpf >= 10) 01378 data = INV_FILTER_10HZ; 01379 else 01380 data = INV_FILTER_5HZ; 01381 01382 if (st.chip_cfg.lpf == data) 01383 return 0; 01384 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, &data)) 01385 return -1; 01386 st.chip_cfg.lpf = data; 01387 return 0; 01388 } 01389 01390 /** 01391 * @brief Get sampling rate. 01392 * @param[out] rate Current sampling rate (Hz). 01393 * @return 0 if successful. 01394 */ 01395 int mpu_get_sample_rate(unsigned short *rate) 01396 { 01397 if (st.chip_cfg.dmp_on) 01398 return -1; 01399 else 01400 rate[0] = st.chip_cfg.sample_rate; 01401 return 0; 01402 } 01403 01404 /** 01405 * @brief Set sampling rate. 01406 * Sampling rate must be between 4Hz and 1kHz. 01407 * @param[in] rate Desired sampling rate (Hz). 01408 * @return 0 if successful. 01409 */ 01410 int mpu_set_sample_rate(unsigned short rate) 01411 { 01412 unsigned char data; 01413 01414 if (!(st.chip_cfg.sensors)) 01415 return -1; 01416 01417 if (st.chip_cfg.dmp_on) 01418 return -1; 01419 else { 01420 if (st.chip_cfg.lp_accel_mode) { 01421 if (rate && (rate <= 40)) { 01422 /* Just stay in low-power accel mode. */ 01423 mpu_lp_accel_mode(rate); 01424 return 0; 01425 } 01426 /* Requested rate exceeds the allowed frequencies in LP accel mode, 01427 * switch back to full-power mode. 01428 */ 01429 mpu_lp_accel_mode(0); 01430 } 01431 if (rate < 4) 01432 rate = 4; 01433 else if (rate > 1000) 01434 rate = 1000; 01435 01436 data = 1000 / rate - 1; 01437 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, &data)) 01438 return -1; 01439 01440 st.chip_cfg.sample_rate = 1000 / (1 + data); 01441 01442 #ifdef AK89xx_SECONDARY 01443 mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); 01444 #endif 01445 01446 /* Automatically set LPF to 1/2 sampling rate. */ 01447 mpu_set_lpf(st.chip_cfg.sample_rate >> 1); 01448 return 0; 01449 } 01450 } 01451 01452 /** 01453 * @brief Get compass sampling rate. 01454 * @param[out] rate Current compass sampling rate (Hz). 01455 * @return 0 if successful. 01456 */ 01457 int mpu_get_compass_sample_rate(unsigned short *rate) 01458 { 01459 #ifdef AK89xx_SECONDARY 01460 rate[0] = st.chip_cfg.compass_sample_rate; 01461 return 0; 01462 #else 01463 rate[0] = 0; 01464 return -1; 01465 #endif 01466 } 01467 01468 /** 01469 * @brief Set compass sampling rate. 01470 * The compass on the auxiliary I2C bus is read by the MPU hardware at a 01471 * maximum of 100Hz. The actual rate can be set to a fraction of the gyro 01472 * sampling rate. 01473 * 01474 * \n WARNING: The new rate may be different than what was requested. Call 01475 * mpu_get_compass_sample_rate to check the actual setting. 01476 * @param[in] rate Desired compass sampling rate (Hz). 01477 * @return 0 if successful. 01478 */ 01479 int mpu_set_compass_sample_rate(unsigned short rate) 01480 { 01481 #ifdef AK89xx_SECONDARY 01482 unsigned char div; 01483 if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) 01484 return -1; 01485 01486 div = st.chip_cfg.sample_rate / rate - 1; 01487 if (mpu_hal_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) 01488 return -1; 01489 st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); 01490 return 0; 01491 #else 01492 return -1; 01493 #endif 01494 } 01495 01496 /** 01497 * @brief Get gyro sensitivity scale factor. 01498 * @param[out] sens Conversion from hardware units to dps. 01499 * @return 0 if successful. 01500 */ 01501 int mpu_get_gyro_sens(float *sens) 01502 { 01503 switch (st.chip_cfg.gyro_fsr) { 01504 case INV_FSR_250DPS: 01505 sens[0] = 131.f; 01506 break; 01507 case INV_FSR_500DPS: 01508 sens[0] = 65.5f; 01509 break; 01510 case INV_FSR_1000DPS: 01511 sens[0] = 32.8f; 01512 break; 01513 case INV_FSR_2000DPS: 01514 sens[0] = 16.4f; 01515 break; 01516 default: 01517 return -1; 01518 } 01519 return 0; 01520 } 01521 01522 /** 01523 * @brief Get accel sensitivity scale factor. 01524 * @param[out] sens Conversion from hardware units to g's. 01525 * @return 0 if successful. 01526 */ 01527 int mpu_get_accel_sens(unsigned short *sens) 01528 { 01529 switch (st.chip_cfg.accel_fsr) { 01530 case INV_FSR_2G: 01531 sens[0] = 16384; 01532 break; 01533 case INV_FSR_4G: 01534 sens[0] = 8092; 01535 break; 01536 case INV_FSR_8G: 01537 sens[0] = 4096; 01538 break; 01539 case INV_FSR_16G: 01540 sens[0] = 2048; 01541 break; 01542 default: 01543 return -1; 01544 } 01545 if (st.chip_cfg.accel_half) 01546 sens[0] >>= 1; 01547 return 0; 01548 } 01549 01550 /** 01551 * @brief Get current FIFO configuration. 01552 * @e sensors can contain a combination of the following flags: 01553 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01554 * \n INV_XYZ_GYRO 01555 * \n INV_XYZ_ACCEL 01556 * @param[out] sensors Mask of sensors in FIFO. 01557 * @return 0 if successful. 01558 */ 01559 int mpu_get_fifo_config(unsigned char *sensors) 01560 { 01561 sensors[0] = st.chip_cfg.fifo_enable; 01562 return 0; 01563 } 01564 01565 /** 01566 * @brief Select which sensors are pushed to FIFO. 01567 * @e sensors can contain a combination of the following flags: 01568 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01569 * \n INV_XYZ_GYRO 01570 * \n INV_XYZ_ACCEL 01571 * @param[in] sensors Mask of sensors to push to FIFO. 01572 * @return 0 if successful. 01573 */ 01574 int mpu_configure_fifo(unsigned char sensors) 01575 { 01576 unsigned char prev; 01577 int result = 0; 01578 01579 /* Compass data isn't going into the FIFO. Stop trying. */ 01580 sensors &= ~INV_XYZ_COMPASS; 01581 01582 if (st.chip_cfg.dmp_on) 01583 return 0; 01584 else { 01585 if (!(st.chip_cfg.sensors)) 01586 return -1; 01587 prev = st.chip_cfg.fifo_enable; 01588 st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; 01589 if (st.chip_cfg.fifo_enable != sensors) 01590 /* You're not getting what you asked for. Some sensors are 01591 * asleep. 01592 */ 01593 result = -1; 01594 else 01595 result = 0; 01596 if (sensors || st.chip_cfg.lp_accel_mode) 01597 set_int_enable(1); 01598 else 01599 set_int_enable(0); 01600 if (sensors) { 01601 if (mpu_reset_fifo()) { 01602 st.chip_cfg.fifo_enable = prev; 01603 return -1; 01604 } 01605 } 01606 } 01607 01608 return result; 01609 } 01610 01611 /** 01612 * @brief Get current power state. 01613 * @param[in] power_on 1 if turned on, 0 if suspended. 01614 * @return 0 if successful. 01615 */ 01616 int mpu_get_power_state(unsigned char *power_on) 01617 { 01618 if (st.chip_cfg.sensors) 01619 power_on[0] = 1; 01620 else 01621 power_on[0] = 0; 01622 return 0; 01623 } 01624 01625 /** 01626 * @brief Turn specific sensors on/off. 01627 * @e sensors can contain a combination of the following flags: 01628 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01629 * \n INV_XYZ_GYRO 01630 * \n INV_XYZ_ACCEL 01631 * \n INV_XYZ_COMPASS 01632 * @param[in] sensors Mask of sensors to wake. 01633 * @return 0 if successful. 01634 */ 01635 int mpu_set_sensors(unsigned char sensors) 01636 { 01637 unsigned char data; 01638 #ifdef AK89xx_SECONDARY 01639 unsigned char user_ctrl; 01640 #endif 01641 01642 if (sensors & INV_XYZ_GYRO) 01643 data = INV_CLK_PLL; 01644 else if (sensors) 01645 data = 0; 01646 else 01647 data = BIT_SLEEP; 01648 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { 01649 st.chip_cfg.sensors = 0; 01650 return -1; 01651 } 01652 st.chip_cfg.clk_src = data & ~BIT_SLEEP; 01653 01654 data = 0; 01655 if (!(sensors & INV_X_GYRO)) 01656 data |= BIT_STBY_XG; 01657 if (!(sensors & INV_Y_GYRO)) 01658 data |= BIT_STBY_YG; 01659 if (!(sensors & INV_Z_GYRO)) 01660 data |= BIT_STBY_ZG; 01661 if (!(sensors & INV_XYZ_ACCEL)) 01662 data |= BIT_STBY_XYZA; 01663 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { 01664 st.chip_cfg.sensors = 0; 01665 return -1; 01666 } 01667 01668 if (sensors && (sensors != INV_XYZ_ACCEL)) 01669 /* Latched interrupts only used in LP accel mode. */ 01670 mpu_set_int_latched(0); 01671 01672 #ifdef AK89xx_SECONDARY 01673 #ifdef AK89xx_BYPASS 01674 if (sensors & INV_XYZ_COMPASS) 01675 mpu_set_bypass(1); 01676 else 01677 mpu_set_bypass(0); 01678 #else 01679 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01680 return -1; 01681 /* Handle AKM power management. */ 01682 if (sensors & INV_XYZ_COMPASS) { 01683 data = AKM_SINGLE_MEASUREMENT; 01684 user_ctrl |= BIT_AUX_IF_EN; 01685 } else { 01686 data = AKM_POWER_DOWN; 01687 user_ctrl &= ~BIT_AUX_IF_EN; 01688 } 01689 if (st.chip_cfg.dmp_on) 01690 user_ctrl |= BIT_DMP_EN; 01691 else 01692 user_ctrl &= ~BIT_DMP_EN; 01693 if (mpu_hal_write(st.hw->addr, st.reg->s1_do, 1, &data)) 01694 return -1; 01695 /* Enable/disable I2C master mode. */ 01696 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01697 return -1; 01698 #endif 01699 #endif 01700 01701 st.chip_cfg.sensors = sensors; 01702 st.chip_cfg.lp_accel_mode = 0; 01703 delay_ms(50); 01704 return 0; 01705 } 01706 01707 /** 01708 * @brief Read the MPU interrupt status registers. 01709 * @param[out] status Mask of interrupt bits. 01710 * @return 0 if successful. 01711 */ 01712 int mpu_get_int_status(short *status) 01713 { 01714 unsigned char tmp[2]; 01715 if (!st.chip_cfg.sensors) 01716 return -1; 01717 if (mpu_hal_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) 01718 return -1; 01719 status[0] = (tmp[0] << 8) | tmp[1]; 01720 return 0; 01721 } 01722 01723 /** 01724 * @brief Get one packet from the FIFO. 01725 * If @e sensors does not contain a particular sensor, disregard the data 01726 * returned to that pointer. 01727 * \n @e sensors can contain a combination of the following flags: 01728 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01729 * \n INV_XYZ_GYRO 01730 * \n INV_XYZ_ACCEL 01731 * \n If the FIFO has no new data, @e sensors will be zero. 01732 * \n If the FIFO is disabled, @e sensors will be zero and this function will 01733 * return a non-zero error code. 01734 * @param[out] gyro Gyro data in hardware units. 01735 * @param[out] accel Accel data in hardware units. 01736 * @param[out] timestamp Timestamp in milliseconds. 01737 * @param[out] sensors Mask of sensors read from FIFO. 01738 * @param[out] more Number of remaining packets. 01739 * @return 0 if successful. 01740 */ 01741 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, 01742 unsigned char *sensors, unsigned char *more) 01743 { 01744 /* Assumes maximum packet size is gyro (6) + accel (6). */ 01745 unsigned char data[MAX_PACKET_LENGTH]; 01746 unsigned char packet_size = 0; 01747 unsigned short fifo_count, index = 0; 01748 01749 if (st.chip_cfg.dmp_on) 01750 return -1; 01751 01752 sensors[0] = 0; 01753 if (!st.chip_cfg.sensors) 01754 return -1; 01755 if (!st.chip_cfg.fifo_enable) 01756 return -1; 01757 01758 if (st.chip_cfg.fifo_enable & INV_X_GYRO) 01759 packet_size += 2; 01760 if (st.chip_cfg.fifo_enable & INV_Y_GYRO) 01761 packet_size += 2; 01762 if (st.chip_cfg.fifo_enable & INV_Z_GYRO) 01763 packet_size += 2; 01764 if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) 01765 packet_size += 6; 01766 01767 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 01768 return -1; 01769 fifo_count = (data[0] << 8) | data[1]; 01770 if (fifo_count < packet_size) 01771 return 0; 01772 // log_i("FIFO count: %hd\n", fifo_count); 01773 if (fifo_count > (st.hw->max_fifo >> 1)) { 01774 /* FIFO is 50% full, better check overflow bit. */ 01775 if (mpu_hal_read(st.hw->addr, st.reg->int_status, 1, data)) 01776 return -1; 01777 if (data[0] & BIT_FIFO_OVERFLOW) { 01778 mpu_reset_fifo(); 01779 return -2; 01780 } 01781 } 01782 get_ms((unsigned long*)timestamp); 01783 01784 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) 01785 return -1; 01786 more[0] = fifo_count / packet_size - 1; 01787 sensors[0] = 0; 01788 01789 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { 01790 accel[0] = (data[index+0] << 8) | data[index+1]; 01791 accel[1] = (data[index+2] << 8) | data[index+3]; 01792 accel[2] = (data[index+4] << 8) | data[index+5]; 01793 sensors[0] |= INV_XYZ_ACCEL; 01794 index += 6; 01795 } 01796 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { 01797 gyro[0] = (data[index+0] << 8) | data[index+1]; 01798 sensors[0] |= INV_X_GYRO; 01799 index += 2; 01800 } 01801 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { 01802 gyro[1] = (data[index+0] << 8) | data[index+1]; 01803 sensors[0] |= INV_Y_GYRO; 01804 index += 2; 01805 } 01806 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { 01807 gyro[2] = (data[index+0] << 8) | data[index+1]; 01808 sensors[0] |= INV_Z_GYRO; 01809 index += 2; 01810 } 01811 01812 return 0; 01813 } 01814 01815 /** 01816 * @brief Get one unparsed packet from the FIFO. 01817 * This function should be used if the packet is to be parsed elsewhere. 01818 * @param[in] length Length of one FIFO packet. 01819 * @param[in] data FIFO packet. 01820 * @param[in] more Number of remaining packets. 01821 */ 01822 int mpu_read_fifo_stream(unsigned short length, unsigned char *data, 01823 unsigned char *more) 01824 { 01825 unsigned char tmp[2]; 01826 unsigned short fifo_count; 01827 if (!st.chip_cfg.dmp_on) 01828 return -1; 01829 if (!st.chip_cfg.sensors) 01830 return -1; 01831 01832 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) 01833 return -1; 01834 fifo_count = (tmp[0] << 8) | tmp[1]; 01835 if (fifo_count < length) { 01836 more[0] = 0; 01837 return -1; 01838 } 01839 if (fifo_count > (st.hw->max_fifo >> 1)) { 01840 /* FIFO is 50% full, better check overflow bit. */ 01841 if (mpu_hal_read(st.hw->addr, st.reg->int_status, 1, tmp)) 01842 return -1; 01843 if (tmp[0] & BIT_FIFO_OVERFLOW) { 01844 mpu_reset_fifo(); 01845 return -2; 01846 } 01847 } 01848 01849 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, length, data)) 01850 return -1; 01851 more[0] = fifo_count / length - 1; 01852 return 0; 01853 } 01854 01855 /** 01856 * @brief Set device to bypass mode. 01857 * @param[in] bypass_on 1 to enable bypass mode. 01858 * @return 0 if successful. 01859 */ 01860 int mpu_set_bypass(unsigned char bypass_on) 01861 { 01862 unsigned char tmp; 01863 01864 if (st.chip_cfg.bypass_mode == bypass_on) 01865 return 0; 01866 01867 if (bypass_on) { 01868 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01869 return -1; 01870 tmp &= ~BIT_AUX_IF_EN; 01871 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01872 return -1; 01873 delay_ms(3); 01874 tmp = BIT_BYPASS_EN; 01875 if (st.chip_cfg.active_low_int) 01876 tmp |= BIT_ACTL; 01877 if (st.chip_cfg.latched_int) 01878 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 01879 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01880 return -1; 01881 } else { 01882 /* Enable I2C master mode if compass is being used. */ 01883 if (mpu_hal_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01884 return -1; 01885 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 01886 tmp |= BIT_AUX_IF_EN; 01887 else 01888 tmp &= ~BIT_AUX_IF_EN; 01889 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01890 return -1; 01891 delay_ms(3); 01892 if (st.chip_cfg.active_low_int) 01893 tmp = BIT_ACTL; 01894 else 01895 tmp = 0; 01896 if (st.chip_cfg.latched_int) 01897 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 01898 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01899 return -1; 01900 } 01901 st.chip_cfg.bypass_mode = bypass_on; 01902 return 0; 01903 } 01904 01905 /** 01906 * @brief Set interrupt level. 01907 * @param[in] active_low 1 for active low, 0 for active high. 01908 * @return 0 if successful. 01909 */ 01910 int mpu_set_int_level(unsigned char active_low) 01911 { 01912 st.chip_cfg.active_low_int = active_low; 01913 return 0; 01914 } 01915 01916 /** 01917 * @brief Enable latched interrupts. 01918 * Any MPU register will clear the interrupt. 01919 * @param[in] enable 1 to enable, 0 to disable. 01920 * @return 0 if successful. 01921 */ 01922 int mpu_set_int_latched(unsigned char enable) 01923 { 01924 unsigned char tmp; 01925 if (st.chip_cfg.latched_int == enable) 01926 return 0; 01927 01928 if (enable) 01929 tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; 01930 else 01931 tmp = 0; 01932 if (st.chip_cfg.bypass_mode) 01933 tmp |= BIT_BYPASS_EN; 01934 if (st.chip_cfg.active_low_int) 01935 tmp |= BIT_ACTL; 01936 if (mpu_hal_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01937 return -1; 01938 st.chip_cfg.latched_int = enable; 01939 return 0; 01940 } 01941 01942 #ifdef MPU6050 01943 static int get_accel_prod_shift(float *st_shift) 01944 { 01945 unsigned char tmp[4], shift_code[3], ii; 01946 01947 if (mpu_hal_read(st.hw->addr, 0x0D, 4, tmp)) 01948 return 0x07; 01949 01950 shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); 01951 shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); 01952 shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); 01953 for (ii = 0; ii < 3; ii++) { 01954 if (!shift_code[ii]) { 01955 st_shift[ii] = 0.f; 01956 continue; 01957 } 01958 /* Equivalent to.. 01959 * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) 01960 */ 01961 st_shift[ii] = 0.34f; 01962 while (--shift_code[ii]) 01963 st_shift[ii] *= 1.034f; 01964 } 01965 return 0; 01966 } 01967 01968 static int accel_self_test(long *bias_regular, long *bias_st) 01969 { 01970 int jj, result = 0; 01971 float st_shift[3], st_shift_cust, st_shift_var; 01972 01973 get_accel_prod_shift(st_shift); 01974 for(jj = 0; jj < 3; jj++) { 01975 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 01976 if (st_shift[jj]) { 01977 st_shift_var = st_shift_cust / st_shift[jj] - 1.f; 01978 if (fabs(st_shift_var) > test.max_accel_var) 01979 result |= 1 << jj; 01980 } else if ((st_shift_cust < test.min_g) || 01981 (st_shift_cust > test.max_g)) 01982 result |= 1 << jj; 01983 } 01984 01985 return result; 01986 } 01987 01988 static int gyro_self_test(long *bias_regular, long *bias_st) 01989 { 01990 int jj, result = 0; 01991 unsigned char tmp[3]; 01992 float st_shift, st_shift_cust, st_shift_var; 01993 01994 if (mpu_hal_read(st.hw->addr, 0x0D, 3, tmp)) 01995 return 0x07; 01996 01997 tmp[0] &= 0x1F; 01998 tmp[1] &= 0x1F; 01999 tmp[2] &= 0x1F; 02000 02001 for (jj = 0; jj < 3; jj++) { 02002 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 02003 if (tmp[jj]) { 02004 st_shift = 3275.f / test.gyro_sens; 02005 while (--tmp[jj]) 02006 st_shift *= 1.046f; 02007 st_shift_var = st_shift_cust / st_shift - 1.f; 02008 if (fabs(st_shift_var) > test.max_gyro_var) 02009 result |= 1 << jj; 02010 } else if ((st_shift_cust < test.min_dps) || 02011 (st_shift_cust > test.max_dps)) 02012 result |= 1 << jj; 02013 } 02014 return result; 02015 } 02016 02017 #endif 02018 #ifdef AK89xx_SECONDARY 02019 static int compass_self_test(void) 02020 { 02021 int result = 0x07; 02022 02023 #ifndef MPU_USE_SPI 02024 unsigned char tmp[6]; 02025 unsigned char tries = 10; 02026 short data; 02027 02028 mpu_set_bypass(1); 02029 02030 tmp[0] = AKM_POWER_DOWN; 02031 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 02032 return 0x07; 02033 tmp[0] = AKM_BIT_SELF_TEST; 02034 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) 02035 goto AKM_restore; 02036 tmp[0] = AKM_MODE_SELF_TEST; 02037 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 02038 goto AKM_restore; 02039 02040 do { 02041 delay_ms(10); 02042 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) 02043 goto AKM_restore; 02044 if (tmp[0] & AKM_DATA_READY) 02045 break; 02046 } while (tries--); 02047 if (!(tmp[0] & AKM_DATA_READY)) 02048 goto AKM_restore; 02049 02050 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) 02051 goto AKM_restore; 02052 02053 result = 0; 02054 #if defined MPU9150 02055 data = (short)(tmp[1] << 8) | tmp[0]; 02056 if ((data > 100) || (data < -100)) 02057 result |= 0x01; 02058 data = (short)(tmp[3] << 8) | tmp[2]; 02059 if ((data > 100) || (data < -100)) 02060 result |= 0x02; 02061 data = (short)(tmp[5] << 8) | tmp[4]; 02062 if ((data > -300) || (data < -1000)) 02063 result |= 0x04; 02064 #elif defined MPU9250 02065 data = (short)(tmp[1] << 8) | tmp[0]; 02066 if ((data > 200) || (data < -200)) 02067 result |= 0x01; 02068 data = (short)(tmp[3] << 8) | tmp[2]; 02069 if ((data > 200) || (data < -200)) 02070 result |= 0x02; 02071 data = (short)(tmp[5] << 8) | tmp[4]; 02072 if ((data > -800) || (data < -3200)) 02073 result |= 0x04; 02074 #endif 02075 AKM_restore: 02076 tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; 02077 mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); 02078 tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; 02079 mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); 02080 mpu_set_bypass(0); 02081 #else 02082 // to do - compass self test through spi interface 02083 #endif 02084 02085 return result; 02086 } 02087 #endif 02088 02089 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 02090 { 02091 unsigned char data[MAX_PACKET_LENGTH]; 02092 unsigned char packet_count, ii; 02093 unsigned short fifo_count; 02094 02095 data[0] = 0x01; 02096 data[1] = 0; 02097 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 02098 return -1; 02099 delay_ms(200); 02100 data[0] = 0; 02101 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data)) 02102 return -1; 02103 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02104 return -1; 02105 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 02106 return -1; 02107 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02108 return -1; 02109 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02110 return -1; 02111 data[0] = BIT_FIFO_RST | BIT_DMP_RST; 02112 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02113 return -1; 02114 delay_ms(15); 02115 data[0] = st.test->reg_lpf; 02116 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, data)) 02117 return -1; 02118 data[0] = st.test->reg_rate_div; 02119 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, data)) 02120 return -1; 02121 if (hw_test) 02122 data[0] = st.test->reg_gyro_fsr | 0xE0; 02123 else 02124 data[0] = st.test->reg_gyro_fsr; 02125 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) 02126 return -1; 02127 02128 if (hw_test) 02129 data[0] = st.test->reg_accel_fsr | 0xE0; 02130 else 02131 data[0] = test.reg_accel_fsr; 02132 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 02133 return -1; 02134 if (hw_test) 02135 delay_ms(200); 02136 02137 /* Fill FIFO for test.wait_ms milliseconds. */ 02138 data[0] = BIT_FIFO_EN; 02139 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02140 return -1; 02141 02142 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; 02143 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02144 return -1; 02145 delay_ms(test.wait_ms); 02146 data[0] = 0; 02147 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02148 return -1; 02149 02150 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 02151 return -1; 02152 02153 fifo_count = (data[0] << 8) | data[1]; 02154 packet_count = fifo_count / MAX_PACKET_LENGTH; 02155 gyro[0] = gyro[1] = gyro[2] = 0; 02156 accel[0] = accel[1] = accel[2] = 0; 02157 02158 for (ii = 0; ii < packet_count; ii++) { 02159 short accel_cur[3], gyro_cur[3]; 02160 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) 02161 return -1; 02162 accel_cur[0] = ((short)data[0] << 8) | data[1]; 02163 accel_cur[1] = ((short)data[2] << 8) | data[3]; 02164 accel_cur[2] = ((short)data[4] << 8) | data[5]; 02165 accel[0] += (long)accel_cur[0]; 02166 accel[1] += (long)accel_cur[1]; 02167 accel[2] += (long)accel_cur[2]; 02168 gyro_cur[0] = (((short)data[6] << 8) | data[7]); 02169 gyro_cur[1] = (((short)data[8] << 8) | data[9]); 02170 gyro_cur[2] = (((short)data[10] << 8) | data[11]); 02171 gyro[0] += (long)gyro_cur[0]; 02172 gyro[1] += (long)gyro_cur[1]; 02173 gyro[2] += (long)gyro_cur[2]; 02174 } 02175 #ifdef EMPL_NO_64BIT 02176 gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); 02177 gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); 02178 gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); 02179 if (has_accel) { 02180 accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / 02181 packet_count); 02182 accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / 02183 packet_count); 02184 accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / 02185 packet_count); 02186 /* Don't remove gravity! */ 02187 accel[2] -= 65536L; 02188 } 02189 #else 02190 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); 02191 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); 02192 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); 02193 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / 02194 packet_count); 02195 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / 02196 packet_count); 02197 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / 02198 packet_count); 02199 /* Don't remove gravity! */ 02200 if (accel[2] > 0L) 02201 accel[2] -= 65536L; 02202 else 02203 accel[2] += 65536L; 02204 #endif 02205 02206 return 0; 02207 } 02208 02209 #ifdef MPU6500 02210 #define REG_6500_XG_ST_DATA 0x0 02211 #define REG_6500_XA_ST_DATA 0xD 02212 static const unsigned short mpu_6500_st_tb[256] = { 02213 2620,2646,2672,2699,2726,2753,2781,2808, //7 02214 2837,2865,2894,2923,2952,2981,3011,3041, //15 02215 3072,3102,3133,3165,3196,3228,3261,3293, //23 02216 3326,3359,3393,3427,3461,3496,3531,3566, //31 02217 3602,3638,3674,3711,3748,3786,3823,3862, //39 02218 3900,3939,3979,4019,4059,4099,4140,4182, //47 02219 4224,4266,4308,4352,4395,4439,4483,4528, //55 02220 4574,4619,4665,4712,4759,4807,4855,4903, //63 02221 4953,5002,5052,5103,5154,5205,5257,5310, //71 02222 5363,5417,5471,5525,5581,5636,5693,5750, //79 02223 5807,5865,5924,5983,6043,6104,6165,6226, //87 02224 6289,6351,6415,6479,6544,6609,6675,6742, //95 02225 6810,6878,6946,7016,7086,7157,7229,7301, //103 02226 7374,7448,7522,7597,7673,7750,7828,7906, //111 02227 7985,8065,8145,8227,8309,8392,8476,8561, //119 02228 8647,8733,8820,8909,8998,9088,9178,9270, 02229 9363,9457,9551,9647,9743,9841,9939,10038, 02230 10139,10240,10343,10446,10550,10656,10763,10870, 02231 10979,11089,11200,11312,11425,11539,11654,11771, 02232 11889,12008,12128,12249,12371,12495,12620,12746, 02233 12874,13002,13132,13264,13396,13530,13666,13802, 02234 13940,14080,14221,14363,14506,14652,14798,14946, 02235 15096,15247,15399,15553,15709,15866,16024,16184, 02236 16346,16510,16675,16842,17010,17180,17352,17526, 02237 17701,17878,18057,18237,18420,18604,18790,18978, 02238 19167,19359,19553,19748,19946,20145,20347,20550, 02239 20756,20963,21173,21385,21598,21814,22033,22253, 02240 22475,22700,22927,23156,23388,23622,23858,24097, 02241 24338,24581,24827,25075,25326,25579,25835,26093, 02242 26354,26618,26884,27153,27424,27699,27976,28255, 02243 28538,28823,29112,29403,29697,29994,30294,30597, 02244 30903,31212,31524,31839,32157,32479,32804,33132 02245 }; 02246 static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug) 02247 { 02248 int i, result = 0, otp_value_zero = 0; 02249 float accel_st_al_min, accel_st_al_max; 02250 float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max; 02251 unsigned char regs[3]; 02252 if (mpu_hal_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) { 02253 if(debug) 02254 log_i("Reading OTP Register Error.\n"); 02255 return 0x07; 02256 } 02257 if(debug) 02258 log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]); 02259 for (i = 0; i < 3; i++) { 02260 if (regs[i] != 0) { 02261 ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; 02262 ct_shift_prod[i] *= 65536.f; 02263 ct_shift_prod[i] /= test.accel_sens; 02264 } 02265 else { 02266 ct_shift_prod[i] = 0; 02267 otp_value_zero = 1; 02268 } 02269 } 02270 if(otp_value_zero == 0) { 02271 if(debug) 02272 log_i("ACCEL:CRITERIA A\n"); 02273 for (i = 0; i < 3; i++) { 02274 st_shift_cust[i] = bias_st[i] - bias_regular[i]; 02275 if(debug) { 02276 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", 02277 st_shift_cust[i]/1.f, bias_regular[i]/1.f, 02278 bias_st[i]/1.f); 02279 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); 02280 } 02281 02282 st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f; 02283 02284 if(debug) 02285 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, 02286 test.max_accel_var/1.f); 02287 02288 if (fabs(st_shift_ratio[i]) > test.max_accel_var) { 02289 if(debug) 02290 log_i("ACCEL Fail Axis = %d\n", i); 02291 result |= 1 << i; //Error condition 02292 } 02293 } 02294 } 02295 else { 02296 /* Self Test Pass/Fail Criteria B */ 02297 accel_st_al_min = test.min_g * 65536.f; 02298 accel_st_al_max = test.max_g * 65536.f; 02299 02300 if(debug) { 02301 log_i("ACCEL:CRITERIA B\r\n"); 02302 log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f); 02303 log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f); 02304 } 02305 02306 for (i = 0; i < 3; i++) { 02307 st_shift_cust[i] = bias_st[i] - bias_regular[i]; 02308 02309 if(debug) 02310 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); 02311 if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) { 02312 if(debug) 02313 log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i); 02314 result |= 1 << i; //Error condition 02315 } 02316 } 02317 } 02318 02319 if(result == 0) { 02320 /* Self Test Pass/Fail Criteria C */ 02321 accel_offset_max = test.max_g_offset * 65536.f; 02322 if(debug) 02323 log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f); 02324 for (i = 0; i < 3; i++) { 02325 if(fabs(bias_regular[i]) > accel_offset_max) { 02326 if(debug) 02327 log_i("FAILED: Accel axis:%d = %d > 500mg\n", i, bias_regular[i]); 02328 result |= 1 << i; //Error condition 02329 } 02330 } 02331 } 02332 02333 return result; 02334 } 02335 02336 static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug) 02337 { 02338 int i, result = 0, otp_value_zero = 0; 02339 float gyro_st_al_max; 02340 float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max; 02341 unsigned char regs[3]; 02342 02343 if (mpu_hal_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) { 02344 if(debug) 02345 log_i("Reading OTP Register Error.\n"); 02346 return 0x07; 02347 } 02348 02349 if(debug) 02350 log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); 02351 02352 for (i = 0; i < 3; i++) { 02353 if (regs[i] != 0) { 02354 ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; 02355 ct_shift_prod[i] *= 65536.f; 02356 ct_shift_prod[i] /= test.gyro_sens; 02357 } 02358 else { 02359 ct_shift_prod[i] = 0; 02360 otp_value_zero = 1; 02361 } 02362 } 02363 02364 if(otp_value_zero == 0) { 02365 if(debug) 02366 log_i("GYRO:CRITERIA A\n"); 02367 /* Self Test Pass/Fail Criteria A */ 02368 for (i = 0; i < 3; i++) { 02369 st_shift_cust[i] = bias_st[i] - bias_regular[i]; 02370 02371 if(debug) { 02372 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", 02373 st_shift_cust[i]/1.f, bias_regular[i]/1.f, 02374 bias_st[i]/1.f); 02375 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); 02376 } 02377 02378 st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i]; 02379 02380 if(debug) 02381 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, 02382 test.max_gyro_var/1.f); 02383 02384 if (fabs(st_shift_ratio[i]) < test.max_gyro_var) { 02385 if(debug) 02386 log_i("Gyro Fail Axis = %d\n", i); 02387 result |= 1 << i; //Error condition 02388 } 02389 } 02390 } 02391 else { 02392 /* Self Test Pass/Fail Criteria B */ 02393 gyro_st_al_max = test.max_dps * 65536.f; 02394 02395 if(debug) { 02396 log_i("GYRO:CRITERIA B\r\n"); 02397 log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f); 02398 } 02399 02400 for (i = 0; i < 3; i++) { 02401 st_shift_cust[i] = bias_st[i] - bias_regular[i]; 02402 02403 if(debug) 02404 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); 02405 if(st_shift_cust[i] < gyro_st_al_max) { 02406 if(debug) 02407 log_i("GYRO FAIL axis:%d greater than 60dps\n", i); 02408 result |= 1 << i; //Error condition 02409 } 02410 } 02411 } 02412 02413 if(result == 0) { 02414 /* Self Test Pass/Fail Criteria C */ 02415 gyro_offset_max = test.min_dps * 65536.f; 02416 if(debug) 02417 log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f); 02418 for (i = 0; i < 3; i++) { 02419 if(fabs(bias_regular[i]) > gyro_offset_max) { 02420 if(debug) 02421 log_i("FAILED: Gyro axis:%d = %d > 20dps\n", i, bias_regular[i]); 02422 result |= 1 << i; //Error condition 02423 } 02424 } 02425 } 02426 return result; 02427 } 02428 02429 static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug) 02430 { 02431 unsigned char data[HWST_MAX_PACKET_LENGTH]; 02432 unsigned char packet_count, ii; 02433 unsigned short fifo_count; 02434 int s = 0, read_size = 0, ind; 02435 02436 data[0] = 0x01; 02437 data[1] = 0; 02438 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 02439 return -1; 02440 delay_ms(200); 02441 data[0] = 0; 02442 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data)) 02443 return -1; 02444 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02445 return -1; 02446 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 02447 return -1; 02448 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02449 return -1; 02450 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02451 return -1; 02452 data[0] = BIT_FIFO_RST | BIT_DMP_RST; 02453 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02454 return -1; 02455 delay_ms(15); 02456 data[0] = st.test->reg_lpf; 02457 if (mpu_hal_write(st.hw->addr, st.reg->lpf, 1, data)) 02458 return -1; 02459 data[0] = st.test->reg_rate_div; 02460 if (mpu_hal_write(st.hw->addr, st.reg->rate_div, 1, data)) 02461 return -1; 02462 if (hw_test) 02463 data[0] = st.test->reg_gyro_fsr | 0xE0; 02464 else 02465 data[0] = st.test->reg_gyro_fsr; 02466 if (mpu_hal_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) 02467 return -1; 02468 02469 if (hw_test) 02470 data[0] = st.test->reg_accel_fsr | 0xE0; 02471 else 02472 data[0] = test.reg_accel_fsr; 02473 if (mpu_hal_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 02474 return -1; 02475 02476 delay_ms(test.wait_ms); //wait 200ms for sensors to stabilize 02477 02478 /* Enable FIFO */ 02479 data[0] = BIT_FIFO_EN; 02480 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02481 return -1; 02482 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; 02483 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02484 return -1; 02485 02486 //initialize the bias return values 02487 gyro[0] = gyro[1] = gyro[2] = 0; 02488 accel[0] = accel[1] = accel[2] = 0; 02489 02490 if(debug) 02491 log_i("Starting Bias Loop Reads\n"); 02492 02493 //start reading samples 02494 while (s < test.packet_thresh) { 02495 delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO 02496 if (mpu_hal_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 02497 return -1; 02498 fifo_count = (data[0] << 8) | data[1]; 02499 packet_count = fifo_count / MAX_PACKET_LENGTH; 02500 if ((test.packet_thresh - s) < packet_count) 02501 packet_count = test.packet_thresh - s; 02502 read_size = packet_count * MAX_PACKET_LENGTH; 02503 02504 //burst read from FIFO 02505 if (mpu_hal_read(st.hw->addr, st.reg->fifo_r_w, read_size, data)) 02506 return -1; 02507 ind = 0; 02508 for (ii = 0; ii < packet_count; ii++) { 02509 short accel_cur[3], gyro_cur[3]; 02510 accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1]; 02511 accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3]; 02512 accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5]; 02513 accel[0] += (long)accel_cur[0]; 02514 accel[1] += (long)accel_cur[1]; 02515 accel[2] += (long)accel_cur[2]; 02516 gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]); 02517 gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]); 02518 gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]); 02519 gyro[0] += (long)gyro_cur[0]; 02520 gyro[1] += (long)gyro_cur[1]; 02521 gyro[2] += (long)gyro_cur[2]; 02522 ind += MAX_PACKET_LENGTH; 02523 } 02524 s += packet_count; 02525 } 02526 02527 if(debug) 02528 log_i("Samples: %d\n", s); 02529 02530 //stop FIFO 02531 data[0] = 0; 02532 if (mpu_hal_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02533 return -1; 02534 02535 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s); 02536 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s); 02537 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s); 02538 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s); 02539 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s); 02540 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s); 02541 /* remove gravity from bias calculation */ 02542 if (accel[2] > 0L) 02543 accel[2] -= 65536L; 02544 else 02545 accel[2] += 65536L; 02546 02547 02548 if(debug) { 02549 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); 02550 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); 02551 } 02552 02553 return 0; 02554 } 02555 /** 02556 * @brief Trigger gyro/accel/compass self-test for MPU6500/MPU9250 02557 * On success/error, the self-test returns a mask representing the sensor(s) 02558 * that failed. For each bit, a one (1) represents a "pass" case; conversely, 02559 * a zero (0) indicates a failure. 02560 * 02561 * \n The mask is defined as follows: 02562 * \n Bit 0: Gyro. 02563 * \n Bit 1: Accel. 02564 * \n Bit 2: Compass. 02565 * 02566 * @param[out] gyro Gyro biases in q16 format. 02567 * @param[out] accel Accel biases (if applicable) in q16 format. 02568 * @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver. 02569 * @return Result mask (see above). 02570 */ 02571 int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) 02572 { 02573 const unsigned char tries = 2; 02574 long gyro_st[3], accel_st[3]; 02575 unsigned char accel_result, gyro_result; 02576 #ifdef AK89xx_SECONDARY 02577 unsigned char compass_result; 02578 #endif 02579 int ii; 02580 02581 int result; 02582 unsigned char accel_fsr, fifo_sensors, sensors_on; 02583 unsigned short gyro_fsr, sample_rate, lpf; 02584 unsigned char dmp_was_on; 02585 02586 02587 02588 if(debug) 02589 log_i("Starting MPU6500 HWST!\r\n"); 02590 02591 if (st.chip_cfg.dmp_on) { 02592 mpu_set_dmp_state(0); 02593 dmp_was_on = 1; 02594 } else 02595 dmp_was_on = 0; 02596 02597 /* Get initial settings. */ 02598 mpu_get_gyro_fsr(&gyro_fsr); 02599 mpu_get_accel_fsr(&accel_fsr); 02600 mpu_get_lpf(&lpf); 02601 mpu_get_sample_rate(&sample_rate); 02602 sensors_on = st.chip_cfg.sensors; 02603 mpu_get_fifo_config(&fifo_sensors); 02604 02605 if(debug) 02606 log_i("Retrieving Biases\r\n"); 02607 02608 for (ii = 0; ii < tries; ii++) 02609 if (!get_st_6500_biases(gyro, accel, 0, debug)) 02610 break; 02611 if (ii == tries) { 02612 /* If we reach this point, we most likely encountered an I2C error. 02613 * We'll just report an error for all three sensors. 02614 */ 02615 if(debug) 02616 log_i("Retrieving Biases Error - possible I2C error\n"); 02617 02618 result = 0; 02619 goto restore; 02620 } 02621 02622 if(debug) 02623 log_i("Retrieving ST Biases\n"); 02624 02625 for (ii = 0; ii < tries; ii++) 02626 if (!get_st_6500_biases(gyro_st, accel_st, 1, debug)) 02627 break; 02628 if (ii == tries) { 02629 02630 if(debug) 02631 log_i("Retrieving ST Biases Error - possible I2C error\n"); 02632 02633 /* Again, probably an I2C error. */ 02634 result = 0; 02635 goto restore; 02636 } 02637 02638 accel_result = accel_6500_self_test(accel, accel_st, debug); 02639 if(debug) 02640 log_i("Accel Self Test Results: %d\n", accel_result); 02641 02642 gyro_result = gyro_6500_self_test(gyro, gyro_st, debug); 02643 if(debug) 02644 log_i("Gyro Self Test Results: %d\n", gyro_result); 02645 02646 result = 0; 02647 if (!gyro_result) 02648 result |= 0x01; 02649 if (!accel_result) 02650 result |= 0x02; 02651 02652 #ifdef AK89xx_SECONDARY 02653 compass_result = compass_self_test(); 02654 if(debug) 02655 log_i("Compass Self Test Results: %d\n", compass_result); 02656 if (!compass_result) 02657 result |= 0x04; 02658 #else 02659 result |= 0x04; 02660 #endif 02661 restore: 02662 if(debug) 02663 log_i("Exiting HWST\n"); 02664 /* Set to invalid values to ensure no I2C writes are skipped. */ 02665 st.chip_cfg.gyro_fsr = 0xFF; 02666 st.chip_cfg.accel_fsr = 0xFF; 02667 st.chip_cfg.lpf = 0xFF; 02668 st.chip_cfg.sample_rate = 0xFFFF; 02669 st.chip_cfg.sensors = 0xFF; 02670 st.chip_cfg.fifo_enable = 0xFF; 02671 st.chip_cfg.clk_src = INV_CLK_PLL; 02672 mpu_set_gyro_fsr(gyro_fsr); 02673 mpu_set_accel_fsr(accel_fsr); 02674 mpu_set_lpf(lpf); 02675 mpu_set_sample_rate(sample_rate); 02676 mpu_set_sensors(sensors_on); 02677 mpu_configure_fifo(fifo_sensors); 02678 02679 if (dmp_was_on) 02680 mpu_set_dmp_state(1); 02681 02682 return result; 02683 } 02684 #endif 02685 /* 02686 * \n This function must be called with the device either face-up or face-down 02687 * (z-axis is parallel to gravity). 02688 * @param[out] gyro Gyro biases in q16 format. 02689 * @param[out] accel Accel biases (if applicable) in q16 format. 02690 * @return Result mask (see above). 02691 */ 02692 int mpu_run_self_test(long *gyro, long *accel) 02693 { 02694 #ifdef MPU6050 02695 const unsigned char tries = 2; 02696 long gyro_st[3], accel_st[3]; 02697 unsigned char accel_result, gyro_result; 02698 #ifdef AK89xx_SECONDARY 02699 unsigned char compass_result; 02700 #endif 02701 int ii; 02702 #endif 02703 int result; 02704 unsigned char accel_fsr, fifo_sensors, sensors_on; 02705 unsigned short gyro_fsr, sample_rate, lpf; 02706 unsigned char dmp_was_on; 02707 02708 if (st.chip_cfg.dmp_on) { 02709 mpu_set_dmp_state(0); 02710 dmp_was_on = 1; 02711 } else 02712 dmp_was_on = 0; 02713 02714 /* Get initial settings. */ 02715 mpu_get_gyro_fsr(&gyro_fsr); 02716 mpu_get_accel_fsr(&accel_fsr); 02717 mpu_get_lpf(&lpf); 02718 mpu_get_sample_rate(&sample_rate); 02719 sensors_on = st.chip_cfg.sensors; 02720 mpu_get_fifo_config(&fifo_sensors); 02721 02722 /* For older chips, the self-test will be different. */ 02723 #if defined MPU6050 02724 for (ii = 0; ii < tries; ii++) 02725 if (!get_st_biases(gyro, accel, 0)) 02726 break; 02727 if (ii == tries) { 02728 /* If we reach this point, we most likely encountered an I2C error. 02729 * We'll just report an error for all three sensors. 02730 */ 02731 result = 0; 02732 goto restore; 02733 } 02734 for (ii = 0; ii < tries; ii++) 02735 if (!get_st_biases(gyro_st, accel_st, 1)) 02736 break; 02737 if (ii == tries) { 02738 /* Again, probably an I2C error. */ 02739 result = 0; 02740 goto restore; 02741 } 02742 accel_result = accel_self_test(accel, accel_st); 02743 gyro_result = gyro_self_test(gyro, gyro_st); 02744 02745 result = 0; 02746 if (!gyro_result) 02747 result |= 0x01; 02748 if (!accel_result) 02749 result |= 0x02; 02750 02751 #ifdef AK89xx_SECONDARY 02752 compass_result = compass_self_test(); 02753 if (!compass_result) 02754 result |= 0x04; 02755 #else 02756 result |= 0x04; 02757 #endif 02758 restore: 02759 #elif defined MPU6500 02760 /* For now, this function will return a "pass" result for all three sensors 02761 * for compatibility with current test applications. 02762 */ 02763 get_st_biases(gyro, accel, 0); 02764 result = 0x7; 02765 #endif 02766 /* Set to invalid values to ensure no I2C writes are skipped. */ 02767 st.chip_cfg.gyro_fsr = 0xFF; 02768 st.chip_cfg.accel_fsr = 0xFF; 02769 st.chip_cfg.lpf = 0xFF; 02770 st.chip_cfg.sample_rate = 0xFFFF; 02771 st.chip_cfg.sensors = 0xFF; 02772 st.chip_cfg.fifo_enable = 0xFF; 02773 st.chip_cfg.clk_src = INV_CLK_PLL; 02774 mpu_set_gyro_fsr(gyro_fsr); 02775 mpu_set_accel_fsr(accel_fsr); 02776 mpu_set_lpf(lpf); 02777 mpu_set_sample_rate(sample_rate); 02778 mpu_set_sensors(sensors_on); 02779 mpu_configure_fifo(fifo_sensors); 02780 02781 if (dmp_was_on) 02782 mpu_set_dmp_state(1); 02783 02784 return result; 02785 } 02786 02787 /** 02788 * @brief Write to the DMP memory. 02789 * This function prevents I2C writes past the bank boundaries. The DMP memory 02790 * is only accessible when the chip is awake. 02791 * @param[in] mem_addr Memory location (bank << 8 | start address) 02792 * @param[in] length Number of bytes to write. 02793 * @param[in] data Bytes to write to memory. 02794 * @return 0 if successful. 02795 */ 02796 int mpu_write_mem(unsigned short mem_addr, unsigned short length, 02797 unsigned char *data) 02798 { 02799 unsigned char tmp[2]; 02800 02801 if (!data) 02802 return -1; 02803 if (!st.chip_cfg.sensors) 02804 return -1; 02805 02806 tmp[0] = (unsigned char)(mem_addr >> 8); 02807 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02808 02809 /* Check bank boundaries. */ 02810 if (tmp[1] + length > st.hw->bank_size) 02811 return -1; 02812 02813 if (mpu_hal_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02814 return -1; 02815 if (mpu_hal_write(st.hw->addr, st.reg->mem_r_w, length, data)) 02816 return -1; 02817 return 0; 02818 } 02819 02820 /** 02821 * @brief Read from the DMP memory. 02822 * This function prevents I2C reads past the bank boundaries. The DMP memory 02823 * is only accessible when the chip is awake. 02824 * @param[in] mem_addr Memory location (bank << 8 | start address) 02825 * @param[in] length Number of bytes to read. 02826 * @param[out] data Bytes read from memory. 02827 * @return 0 if successful. 02828 */ 02829 int mpu_read_mem(unsigned short mem_addr, unsigned short length, 02830 unsigned char *data) 02831 { 02832 unsigned char tmp[2]; 02833 02834 if (!data) 02835 return -1; 02836 if (!st.chip_cfg.sensors) 02837 return -1; 02838 02839 tmp[0] = (unsigned char)(mem_addr >> 8); 02840 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02841 02842 /* Check bank boundaries. */ 02843 if (tmp[1] + length > st.hw->bank_size) 02844 return -1; 02845 02846 if (mpu_hal_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02847 return -1; 02848 if (mpu_hal_read(st.hw->addr, st.reg->mem_r_w, length, data)) 02849 return -1; 02850 return 0; 02851 } 02852 02853 /** 02854 * @brief Load and verify DMP image. 02855 * @param[in] length Length of DMP image. 02856 * @param[in] firmware DMP code. 02857 * @param[in] start_addr Starting address of DMP code memory. 02858 * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. 02859 * @return 0 if successful. 02860 */ 02861 int mpu_load_firmware(unsigned short length, const unsigned char *firmware, 02862 unsigned short start_addr, unsigned short sample_rate) 02863 { 02864 unsigned short ii; 02865 unsigned short this_write; 02866 /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ 02867 #define LOAD_CHUNK (16) 02868 static unsigned char cur[LOAD_CHUNK], tmp[2]; 02869 02870 if (st.chip_cfg.dmp_loaded) 02871 /* DMP should only be loaded once. */ 02872 return -1; 02873 02874 if (!firmware) 02875 return -1; 02876 for (ii = 0; ii < length; ii += this_write) { 02877 this_write = min(LOAD_CHUNK, length - ii); 02878 if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) 02879 return -1; 02880 if (mpu_read_mem(ii, this_write, cur)) 02881 return -1; 02882 if (memcmp(firmware+ii, cur, this_write)) 02883 return -2; 02884 } 02885 02886 /* Set program start address. */ 02887 tmp[0] = start_addr >> 8; 02888 tmp[1] = start_addr & 0xFF; 02889 if (mpu_hal_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) 02890 return -1; 02891 02892 st.chip_cfg.dmp_loaded = 1; 02893 st.chip_cfg.dmp_sample_rate = sample_rate; 02894 return 0; 02895 } 02896 02897 /** 02898 * @brief Enable/disable DMP support. 02899 * @param[in] enable 1 to turn on the DMP. 02900 * @return 0 if successful. 02901 */ 02902 int mpu_set_dmp_state(unsigned char enable) 02903 { 02904 unsigned char tmp; 02905 if (st.chip_cfg.dmp_on == enable) 02906 return 0; 02907 02908 if (enable) { 02909 if (!st.chip_cfg.dmp_loaded) 02910 return -1; 02911 /* Disable data ready interrupt. */ 02912 set_int_enable(0); 02913 /* Disable bypass mode. */ 02914 mpu_set_bypass(0); 02915 /* Keep constant sample rate, FIFO rate controlled by DMP. */ 02916 mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); 02917 /* Remove FIFO elements. */ 02918 tmp = 0; 02919 mpu_hal_write(st.hw->addr, 0x23, 1, &tmp); 02920 st.chip_cfg.dmp_on = 1; 02921 /* Enable DMP interrupt. */ 02922 set_int_enable(1); 02923 mpu_reset_fifo(); 02924 } else { 02925 /* Disable DMP interrupt. */ 02926 set_int_enable(0); 02927 /* Restore FIFO settings. */ 02928 tmp = st.chip_cfg.fifo_enable; 02929 mpu_hal_write(st.hw->addr, 0x23, 1, &tmp); 02930 st.chip_cfg.dmp_on = 0; 02931 mpu_reset_fifo(); 02932 } 02933 return 0; 02934 } 02935 02936 /** 02937 * @brief Get DMP state. 02938 * @param[out] enabled 1 if enabled. 02939 * @return 0 if successful. 02940 */ 02941 int mpu_get_dmp_state(unsigned char *enabled) 02942 { 02943 enabled[0] = st.chip_cfg.dmp_on; 02944 return 0; 02945 } 02946 02947 #ifdef AK89xx_SECONDARY 02948 /* This initialization is similar to the one in ak8975.c. */ 02949 static int setup_compass(void) 02950 { 02951 unsigned char data[4], akm_addr; 02952 02953 #ifndef MPU_USE_SPI 02954 mpu_set_bypass(1); 02955 02956 /* Find compass. Possible addresses range from 0x0C to 0x0F. */ 02957 for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { 02958 int result; 02959 result = mpu_hal_read(akm_addr, AKM_REG_WHOAMI, 1, data); 02960 if (!result && (data[0] == AKM_WHOAMI)) 02961 break; 02962 } 02963 02964 if (akm_addr > 0x0F) { 02965 /* TODO: Handle this case in all compass-related functions. */ 02966 log_e("Compass not found.\n"); 02967 return -1; 02968 } 02969 02970 st.chip_cfg.compass_addr = akm_addr; 02971 02972 data[0] = AKM_POWER_DOWN; 02973 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02974 return -1; 02975 delay_ms(1); 02976 02977 data[0] = AKM_FUSE_ROM_ACCESS; 02978 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02979 return -1; 02980 delay_ms(1); 02981 02982 /* Get sensitivity adjustment data from fuse ROM. */ 02983 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) 02984 return -1; 02985 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; 02986 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; 02987 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; 02988 02989 data[0] = AKM_POWER_DOWN; 02990 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02991 return -1; 02992 delay_ms(1); 02993 02994 mpu_set_bypass(0); 02995 02996 /* Set up master mode, master clock, and ES bit. */ 02997 data[0] = 0x40; 02998 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02999 return -1; 03000 03001 #else 03002 akm_addr = 0x0C; 03003 st.chip_cfg.compass_addr = akm_addr; 03004 03005 /* Set up master mode, master clock, and ES bit. */ 03006 data[0] = 0x40; 03007 if (mpu_hal_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 03008 return -1; 03009 03010 /* Slave 0 reads from AKM adj registers. */ 03011 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; 03012 if (mpu_hal_write(st.hw->addr, st.reg->s0_addr, 1, data)) 03013 return -1; 03014 03015 /* Compass reads start at this register. */ 03016 data[0] = AKM_REG_ASAX; 03017 if (mpu_hal_write(st.hw->addr, st.reg->s0_reg, 1, data)) 03018 return -1; 03019 03020 /* Enable slave 0, 8-byte reads. */ 03021 data[0] = BIT_SLAVE_EN | 3; 03022 if (mpu_hal_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) 03023 return -1; 03024 03025 if (mpu_hal_read(st.hw->addr, st.reg->raw_compass, 3, data)) 03026 return -1; 03027 03028 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; 03029 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; 03030 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; 03031 03032 #endif 03033 03034 /* Slave 0 reads from AKM data registers. */ 03035 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; 03036 if (mpu_hal_write(st.hw->addr, st.reg->s0_addr, 1, data)) 03037 return -1; 03038 03039 /* Compass reads start at this register. */ 03040 data[0] = AKM_REG_ST1; 03041 if (mpu_hal_write(st.hw->addr, st.reg->s0_reg, 1, data)) 03042 return -1; 03043 03044 /* Enable slave 0, 8-byte reads. */ 03045 data[0] = BIT_SLAVE_EN | 8; 03046 if (mpu_hal_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) 03047 return -1; 03048 03049 /* Slave 1 changes AKM measurement mode. */ 03050 data[0] = st.chip_cfg.compass_addr; 03051 if (mpu_hal_write(st.hw->addr, st.reg->s1_addr, 1, data)) 03052 return -1; 03053 03054 /* AKM measurement mode register. */ 03055 data[0] = AKM_REG_CNTL; 03056 if (mpu_hal_write(st.hw->addr, st.reg->s1_reg, 1, data)) 03057 return -1; 03058 03059 /* Enable slave 1, 1-byte writes. */ 03060 data[0] = BIT_SLAVE_EN | 1; 03061 if (mpu_hal_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) 03062 return -1; 03063 03064 /* Set slave 1 data. */ 03065 data[0] = AKM_SINGLE_MEASUREMENT; 03066 if (mpu_hal_write(st.hw->addr, st.reg->s1_do, 1, data)) 03067 return -1; 03068 03069 /* Trigger slave 0 and slave 1 actions at each sample. */ 03070 data[0] = 0x03; 03071 if (mpu_hal_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) 03072 return -1; 03073 03074 #ifdef MPU9150 03075 /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ 03076 data[0] = BIT_I2C_MST_VDDIO; 03077 if (mpu_hal_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) 03078 return -1; 03079 #endif 03080 03081 return 0; 03082 } 03083 #endif 03084 03085 /** 03086 * @brief Read raw compass data. 03087 * @param[out] data Raw data in hardware units. 03088 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 03089 * @return 0 if successful. 03090 */ 03091 int mpu_get_compass_reg(short *data, unsigned long *timestamp) 03092 { 03093 #ifdef AK89xx_SECONDARY 03094 unsigned char tmp[9]; 03095 03096 if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 03097 return -1; 03098 03099 #ifdef AK89xx_BYPASS 03100 if (mpu_hal_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) 03101 return -1; 03102 tmp[8] = AKM_SINGLE_MEASUREMENT; 03103 if (mpu_hal_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) 03104 return -1; 03105 #else 03106 if (mpu_hal_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) 03107 return -1; 03108 #endif 03109 03110 #if defined AK8975_SECONDARY 03111 /* AK8975 doesn't have the overrun error bit. */ 03112 if (!(tmp[0] & AKM_DATA_READY)) 03113 return -2; 03114 if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) 03115 return -3; 03116 #elif defined AK8963_SECONDARY 03117 /* AK8963 doesn't have the data read error bit. */ 03118 if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) 03119 return -2; 03120 if (tmp[7] & AKM_OVERFLOW) 03121 return -3; 03122 #endif 03123 data[0] = (tmp[2] << 8) | tmp[1]; 03124 data[1] = (tmp[4] << 8) | tmp[3]; 03125 data[2] = (tmp[6] << 8) | tmp[5]; 03126 03127 data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; 03128 data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; 03129 data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; 03130 03131 if (timestamp) 03132 get_ms(timestamp); 03133 return 0; 03134 #else 03135 return -1; 03136 #endif 03137 } 03138 03139 /** 03140 * @brief Get the compass full-scale range. 03141 * @param[out] fsr Current full-scale range. 03142 * @return 0 if successful. 03143 */ 03144 int mpu_get_compass_fsr(unsigned short *fsr) 03145 { 03146 #ifdef AK89xx_SECONDARY 03147 fsr[0] = st.hw->compass_fsr; 03148 return 0; 03149 #else 03150 return -1; 03151 #endif 03152 } 03153 03154 /** 03155 * @brief Enters LP accel motion interrupt mode. 03156 * The behaviour of this feature is very different between the MPU6050 and the 03157 * MPU6500. Each chip's version of this feature is explained below. 03158 * 03159 * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg 03160 * increments. 03161 * 03162 * \n Low-power accel mode supports the following frequencies: 03163 * \n 1.25Hz, 5Hz, 20Hz, 40Hz 03164 * 03165 * \n MPU6500: 03166 * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference 03167 * sample. The hardware monitors the accel data and detects any large change 03168 * over a short period of time. 03169 * 03170 * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg 03171 * increments. 03172 * 03173 * \n MPU6500 Low-power accel mode supports the following frequencies: 03174 * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz 03175 * 03176 * \n\n NOTES: 03177 * \n The driver will round down @e thresh to the nearest supported value if 03178 * an unsupported threshold is selected. 03179 * \n To select a fractional wake-up frequency, round down the value passed to 03180 * @e lpa_freq. 03181 * \n The MPU6500 does not support a delay parameter. If this function is used 03182 * for the MPU6500, the value passed to @e time will be ignored. 03183 * \n To disable this mode, set @e lpa_freq to zero. The driver will restore 03184 * the previous configuration. 03185 * 03186 * @param[in] thresh Motion threshold in mg. 03187 * @param[in] time Duration in milliseconds that the accel data must 03188 * exceed @e thresh before motion is reported. 03189 * @param[in] lpa_freq Minimum sampling rate, or zero to disable. 03190 * @return 0 if successful. 03191 */ 03192 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, 03193 unsigned char lpa_freq) 03194 { 03195 03196 #if defined MPU6500 03197 unsigned char data[3]; 03198 #endif 03199 if (lpa_freq) { 03200 #if defined MPU6500 03201 unsigned char thresh_hw; 03202 03203 /* 1LSb = 4mg. */ 03204 if (thresh > 1020) 03205 thresh_hw = 255; 03206 else if (thresh < 4) 03207 thresh_hw = 1; 03208 else 03209 thresh_hw = thresh >> 2; 03210 #endif 03211 03212 if (!time) 03213 /* Minimum duration must be 1ms. */ 03214 time = 1; 03215 03216 #if defined MPU6500 03217 if (lpa_freq > 640) 03218 /* At this point, the chip has not been re-configured, so the 03219 * function can safely exit. 03220 */ 03221 return -1; 03222 #endif 03223 03224 if (!st.chip_cfg.int_motion_only) { 03225 /* Store current settings for later. */ 03226 if (st.chip_cfg.dmp_on) { 03227 mpu_set_dmp_state(0); 03228 st.chip_cfg.cache.dmp_on = 1; 03229 } else 03230 st.chip_cfg.cache.dmp_on = 0; 03231 mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); 03232 mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); 03233 mpu_get_lpf(&st.chip_cfg.cache.lpf); 03234 mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); 03235 st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; 03236 mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); 03237 } 03238 03239 #if defined MPU6500 03240 /* Disable hardware interrupts. */ 03241 set_int_enable(0); 03242 03243 /* Enter full-power accel-only mode, no FIFO/DMP. */ 03244 data[0] = 0; 03245 data[1] = 0; 03246 data[2] = BIT_STBY_XYZG; 03247 if (mpu_hal_write(st.hw->addr, st.reg->user_ctrl, 3, data)) 03248 goto lp_int_restore; 03249 03250 /* Set motion threshold. */ 03251 data[0] = thresh_hw; 03252 if (mpu_hal_write(st.hw->addr, st.reg->motion_thr, 1, data)) 03253 goto lp_int_restore; 03254 03255 /* Set wake frequency. */ 03256 if (lpa_freq == 1) 03257 data[0] = INV_LPA_1_25HZ; 03258 else if (lpa_freq == 2) 03259 data[0] = INV_LPA_2_5HZ; 03260 else if (lpa_freq <= 5) 03261 data[0] = INV_LPA_5HZ; 03262 else if (lpa_freq <= 10) 03263 data[0] = INV_LPA_10HZ; 03264 else if (lpa_freq <= 20) 03265 data[0] = INV_LPA_20HZ; 03266 else if (lpa_freq <= 40) 03267 data[0] = INV_LPA_40HZ; 03268 else if (lpa_freq <= 80) 03269 data[0] = INV_LPA_80HZ; 03270 else if (lpa_freq <= 160) 03271 data[0] = INV_LPA_160HZ; 03272 else if (lpa_freq <= 320) 03273 data[0] = INV_LPA_320HZ; 03274 else 03275 data[0] = INV_LPA_640HZ; 03276 if (mpu_hal_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) 03277 goto lp_int_restore; 03278 03279 /* Enable motion interrupt (MPU6500 version). */ 03280 data[0] = BITS_WOM_EN; 03281 if (mpu_hal_write(st.hw->addr, st.reg->accel_intel, 1, data)) 03282 goto lp_int_restore; 03283 03284 /* Enable cycle mode. */ 03285 data[0] = BIT_LPA_CYCLE; 03286 if (mpu_hal_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 03287 goto lp_int_restore; 03288 03289 /* Enable interrupt. */ 03290 data[0] = BIT_MOT_INT_EN; 03291 if (mpu_hal_write(st.hw->addr, st.reg->int_enable, 1, data)) 03292 goto lp_int_restore; 03293 03294 st.chip_cfg.int_motion_only = 1; 03295 return 0; 03296 #endif 03297 } else { 03298 /* Don't "restore" the previous state if no state has been saved. */ 03299 int ii; 03300 char *cache_ptr = (char*)&st.chip_cfg.cache; 03301 for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { 03302 if (cache_ptr[ii] != 0) 03303 goto lp_int_restore; 03304 } 03305 /* If we reach this point, motion interrupt mode hasn't been used yet. */ 03306 return -1; 03307 } 03308 lp_int_restore: 03309 /* Set to invalid values to ensure no I2C writes are skipped. */ 03310 st.chip_cfg.gyro_fsr = 0xFF; 03311 st.chip_cfg.accel_fsr = 0xFF; 03312 st.chip_cfg.lpf = 0xFF; 03313 st.chip_cfg.sample_rate = 0xFFFF; 03314 st.chip_cfg.sensors = 0xFF; 03315 st.chip_cfg.fifo_enable = 0xFF; 03316 st.chip_cfg.clk_src = INV_CLK_PLL; 03317 mpu_set_sensors(st.chip_cfg.cache.sensors_on); 03318 mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); 03319 mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); 03320 mpu_set_lpf(st.chip_cfg.cache.lpf); 03321 mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); 03322 mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); 03323 03324 if (st.chip_cfg.cache.dmp_on) 03325 mpu_set_dmp_state(1); 03326 03327 #ifdef MPU6500 03328 /* Disable motion interrupt (MPU6500 version). */ 03329 data[0] = 0; 03330 if (mpu_hal_write(st.hw->addr, st.reg->accel_intel, 1, data)) 03331 goto lp_int_restore; 03332 #endif 03333 03334 st.chip_cfg.int_motion_only = 0; 03335 return 0; 03336 } 03337 03338 /** 03339 * @} 03340 */ 03341
Generated on Wed Jul 13 2022 20:01:33 by
1.7.2