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