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