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