inv_mpu.c D:/gitt/MicrochipFor32/bsp_MPU6050/inv_mpu.c 浏览该文件的文档.00001 /* 00002 $License: 00003 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 00004 See included License.txt for License information. 00005 $ 00006 */ 00020 #include <stdio.h> 00021 #include <stdlib.h> 00022 #include <string.h> 00023 #include <math.h> 00024 #include "inv_mpu.h" 00025 #include "inv_mpu_dmp_motion_driver.h" 00026 #include "mpu6050.h" 00027 #include "usart.h" 00028 #include "varint.h" 00029 00030 #define MPU6050 //定义我们使用的传感器为MPU6050 00031 #define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1) 00032 00033 /* The following functions must be defined for this platform: 00034 * i2c_write(unsigned char slave_addr, unsigned char reg_addr, 00035 * unsigned char length, unsigned char const *data) 00036 * i2c_read(unsigned char slave_addr, unsigned char reg_addr, 00037 * unsigned char length, unsigned char *data) 00038 * delay_ms(unsigned long num_ms) 00039 * get_ms(unsigned long *count) 00040 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) 00041 * labs(long x) 00042 * fabsf(float x) 00043 * min(int a, int b) 00044 */ 00045 #if defined MOTION_DRIVER_TARGET_MSP430 00046 00047 #define i2c_write MPU_Write_Len 00048 #define i2c_read MPU_Read_Len 00049 #define delay_ms HAL_Delay 00050 #define get_ms mget_ms 00051 //static inline int reg_int_cb(struct int_param_s *int_param) 00052 //{ 00053 // return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00054 // int_param->active_low); 00055 //} 00056 void log_none(char *fmt,...){;} 00057 #define log_i log_none //打印信息 00058 #define log_e log_none //打印信息 00059 /* labs is already defined by TI's toolchain. */ 00060 /* fabs is for doubles. fabsf is for floats. */ 00061 #define fabs fabsf 00062 #define min(a,b) ((a<b)?a:b) 00063 #elif defined EMPL_TARGET_MSP430 00064 #include "msp430.h" 00065 #include "msp430_i2c.h" 00066 #include "msp430_clock.h" 00067 #include "msp430_interrupt.h" 00068 #include "log.h" 00069 #define i2c_write msp430_i2c_write 00070 #define i2c_read msp430_i2c_read 00071 #define delay_ms msp430_delay_ms 00072 #define get_ms msp430_get_clock_ms 00073 static inline int reg_int_cb(struct int_param_s *int_param) 00074 { 00075 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00076 int_param->active_low); 00077 } 00078 #define log_i MPL_LOGI 00079 #define log_e MPL_LOGE 00080 /* labs is already defined by TI's toolchain. */ 00081 /* fabs is for doubles. fabsf is for floats. */ 00082 #define fabs fabsf 00083 #define min(a,b) ((a<b)?a:b) 00084 #elif defined EMPL_TARGET_UC3L0 00085 /* Instead of using the standard TWI driver from the ASF library, we're using 00086 * a TWI driver that follows the slave address + register address convention. 00087 */ 00088 #include "twi.h" 00089 #include "delay.h" 00090 #include "sysclk.h" 00091 #include "log.h" 00092 #include "sensors_xplained.h" 00093 #include "uc3l0_clock.h" 00094 #define i2c_write(a, b, c, d) twi_write(a, b, d, c) 00095 #define i2c_read(a, b, c, d) twi_read(a, b, d, c) 00096 /* delay_ms is a function already defined in ASF. */ 00097 #define get_ms uc3l0_get_clock_ms 00098 static inline int reg_int_cb(struct int_param_s *int_param) 00099 { 00100 sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg); 00101 return 0; 00102 } 00103 #define log_i MPL_LOGI 00104 #define log_e MPL_LOGE 00105 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */ 00106 #define labs abs 00107 #define fabs(x) (((x)>0)?(x):-(x)) 00108 #else 00109 #error Gyro driver is missing the system layer implementations. 00110 #endif 00111 00112 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 00113 #error Which gyro are you using? Define MPUxxxx in your compiler options. 00114 #endif 00115 00116 /* Time for some messy macro work. =] 00117 * #define MPU9150 00118 * is equivalent to.. 00119 * #define MPU6050 00120 * #define AK8975_SECONDARY 00121 * 00122 * #define MPU9250 00123 * is equivalent to.. 00124 * #define MPU6500 00125 * #define AK8963_SECONDARY 00126 */ 00127 #if defined MPU9150 00128 #ifndef MPU6050 00129 #define MPU6050 00130 #endif /* #ifndef MPU6050 */ 00131 #if defined AK8963_SECONDARY 00132 #error "MPU9150 and AK8963_SECONDARY cannot both be defined." 00133 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ 00134 #define AK8975_SECONDARY 00135 #endif /* #if defined AK8963_SECONDARY */ 00136 #elif defined MPU9250 /* #if defined MPU9150 */ 00137 #ifndef MPU6500 00138 #define MPU6500 00139 #endif /* #ifndef MPU6500 */ 00140 #if defined AK8975_SECONDARY 00141 #error "MPU9250 and AK8975_SECONDARY cannot both be defined." 00142 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ 00143 #define AK8963_SECONDARY 00144 #endif /* #if defined AK8975_SECONDARY */ 00145 #endif /* #if defined MPU9150 */ 00146 00147 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY 00148 #define AK89xx_SECONDARY 00149 #else 00150 /* #warning "No compass = less profit for Invensense. Lame." */ 00151 #endif 00152 00153 static int set_int_enable(unsigned char enable); 00154 00155 /* Hardware registers needed by driver. */ 00156 struct gyro_reg_s { 00157 unsigned char who_am_i; 00158 unsigned char rate_div; 00159 unsigned char lpf; 00160 unsigned char prod_id; 00161 unsigned char user_ctrl; 00162 unsigned char fifo_en; 00163 unsigned char gyro_cfg; 00164 unsigned char accel_cfg; 00165 // unsigned char accel_cfg2; 00166 // unsigned char lp_accel_odr; 00167 unsigned char motion_thr; 00168 unsigned char motion_dur; 00169 unsigned char fifo_count_h; 00170 unsigned char fifo_r_w; 00171 unsigned char raw_gyro; 00172 unsigned char raw_accel; 00173 unsigned char temp; 00174 unsigned char int_enable; 00175 unsigned char dmp_int_status; 00176 unsigned char int_status; 00177 // unsigned char accel_intel; 00178 unsigned char pwr_mgmt_1; 00179 unsigned char pwr_mgmt_2; 00180 unsigned char int_pin_cfg; 00181 unsigned char mem_r_w; 00182 unsigned char accel_offs; 00183 unsigned char i2c_mst; 00184 unsigned char bank_sel; 00185 unsigned char mem_start_addr; 00186 unsigned char prgm_start_h; 00187 #if defined AK89xx_SECONDARY 00188 unsigned char s0_addr; 00189 unsigned char s0_reg; 00190 unsigned char s0_ctrl; 00191 unsigned char s1_addr; 00192 unsigned char s1_reg; 00193 unsigned char s1_ctrl; 00194 unsigned char s4_ctrl; 00195 unsigned char s0_do; 00196 unsigned char s1_do; 00197 unsigned char i2c_delay_ctrl; 00198 unsigned char raw_compass; 00199 /* The I2C_MST_VDDIO bit is in this register. */ 00200 unsigned char yg_offs_tc; 00201 #endif 00202 }; 00203 00204 /* Information specific to a particular device. */ 00205 struct hw_s { 00206 unsigned char addr; 00207 unsigned short max_fifo; 00208 unsigned char num_reg; 00209 unsigned short temp_sens; 00210 short temp_offset; 00211 unsigned short bank_size; 00212 #if defined AK89xx_SECONDARY 00213 unsigned short compass_fsr; 00214 #endif 00215 }; 00216 00217 /* When entering motion interrupt mode, the driver keeps track of the 00218 * previous state so that it can be restored at a later time. 00219 * TODO: This is tacky. Fix it. 00220 */ 00221 struct motion_int_cache_s { 00222 unsigned short gyro_fsr; 00223 unsigned char accel_fsr; 00224 unsigned short lpf; 00225 unsigned short sample_rate; 00226 unsigned char sensors_on; 00227 unsigned char fifo_sensors; 00228 unsigned char dmp_on; 00229 }; 00230 00231 /* Cached chip configuration data. 00232 * TODO: A lot of these can be handled with a bitmask. 00233 */ 00234 struct chip_cfg_s { 00235 /* Matches gyro_cfg >> 3 & 0x03 */ 00236 unsigned char gyro_fsr; 00237 /* Matches accel_cfg >> 3 & 0x03 */ 00238 unsigned char accel_fsr; 00239 /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ 00240 unsigned char sensors; 00241 /* Matches config register. */ 00242 unsigned char lpf; 00243 unsigned char clk_src; 00244 /* Sample rate, NOT rate divider. */ 00245 unsigned short sample_rate; 00246 /* Matches fifo_en register. */ 00247 unsigned char fifo_enable; 00248 /* Matches int enable register. */ 00249 unsigned char int_enable; 00250 /* 1 if devices on auxiliary I2C bus appear on the primary. */ 00251 unsigned char bypass_mode; 00252 /* 1 if half-sensitivity. 00253 * NOTE: This doesn't belong here, but everything else in hw_s is const, 00254 * and this allows us to save some precious RAM. 00255 */ 00256 unsigned char accel_half; 00257 /* 1 if device in low-power accel-only mode. */ 00258 unsigned char lp_accel_mode; 00259 /* 1 if interrupts are only triggered on motion events. */ 00260 unsigned char int_motion_only; 00261 struct motion_int_cache_s cache; 00262 /* 1 for active low interrupts. */ 00263 unsigned char active_low_int; 00264 /* 1 for latched interrupts. */ 00265 unsigned char latched_int; 00266 /* 1 if DMP is enabled. */ 00267 unsigned char dmp_on; 00268 /* Ensures that DMP will only be loaded once. */ 00269 unsigned char dmp_loaded; 00270 /* Sampling rate used when DMP is enabled. */ 00271 unsigned short dmp_sample_rate; 00272 #ifdef AK89xx_SECONDARY 00273 /* Compass sample rate. */ 00274 unsigned short compass_sample_rate; 00275 unsigned char compass_addr; 00276 short mag_sens_adj[3]; 00277 #endif 00278 }; 00279 00280 /* Information for self-test. */ 00281 struct test_s { 00282 unsigned long gyro_sens; 00283 unsigned long accel_sens; 00284 unsigned char reg_rate_div; 00285 unsigned char reg_lpf; 00286 unsigned char reg_gyro_fsr; 00287 unsigned char reg_accel_fsr; 00288 unsigned short wait_ms; 00289 unsigned char packet_thresh; 00290 float min_dps; 00291 float max_dps; 00292 float max_gyro_var; 00293 float min_g; 00294 float max_g; 00295 float max_accel_var; 00296 }; 00297 00298 /* Gyro driver state variables. */ 00299 struct gyro_state_s { 00300 const struct gyro_reg_s *reg; 00301 const struct hw_s *hw; 00302 struct chip_cfg_s chip_cfg; 00303 const struct test_s *test; 00304 }; 00305 00306 /* Filter configurations. */ 00307 enum lpf_e { 00308 INV_FILTER_256HZ_NOLPF2 = 0, 00309 INV_FILTER_188HZ, 00310 INV_FILTER_98HZ, 00311 INV_FILTER_42HZ, 00312 INV_FILTER_20HZ, 00313 INV_FILTER_10HZ, 00314 INV_FILTER_5HZ, 00315 INV_FILTER_2100HZ_NOLPF, 00316 NUM_FILTER 00317 }; 00318 00319 /* Full scale ranges. */ 00320 enum gyro_fsr_e { 00321 INV_FSR_250DPS = 0, 00322 INV_FSR_500DPS, 00323 INV_FSR_1000DPS, 00324 INV_FSR_2000DPS, 00325 NUM_GYRO_FSR 00326 }; 00327 00328 /* Full scale ranges. */ 00329 enum accel_fsr_e { 00330 INV_FSR_2G = 0, 00331 INV_FSR_4G, 00332 INV_FSR_8G, 00333 INV_FSR_16G, 00334 NUM_ACCEL_FSR 00335 }; 00336 00337 /* Clock sources. */ 00338 enum clock_sel_e { 00339 INV_CLK_INTERNAL = 0, 00340 INV_CLK_PLL, 00341 NUM_CLK 00342 }; 00343 00344 /* Low-power accel wakeup rates. */ 00345 enum lp_accel_rate_e { 00346 #if defined MPU6050 00347 INV_LPA_1_25HZ, 00348 INV_LPA_5HZ, 00349 INV_LPA_20HZ, 00350 INV_LPA_40HZ 00351 #elif defined MPU6500 00352 INV_LPA_0_3125HZ, 00353 INV_LPA_0_625HZ, 00354 INV_LPA_1_25HZ, 00355 INV_LPA_2_5HZ, 00356 INV_LPA_5HZ, 00357 INV_LPA_10HZ, 00358 INV_LPA_20HZ, 00359 INV_LPA_40HZ, 00360 INV_LPA_80HZ, 00361 INV_LPA_160HZ, 00362 INV_LPA_320HZ, 00363 INV_LPA_640HZ 00364 #endif 00365 }; 00366 00367 #define BIT_I2C_MST_VDDIO (0x80) 00368 #define BIT_FIFO_EN (0x40) 00369 #define BIT_DMP_EN (0x80) 00370 #define BIT_FIFO_RST (0x04) 00371 #define BIT_DMP_RST (0x08) 00372 #define BIT_FIFO_OVERFLOW (0x10) 00373 #define BIT_DATA_RDY_EN (0x01) 00374 #define BIT_DMP_INT_EN (0x02) 00375 #define BIT_MOT_INT_EN (0x40) 00376 #define BITS_FSR (0x18) 00377 #define BITS_LPF (0x07) 00378 #define BITS_HPF (0x07) 00379 #define BITS_CLK (0x07) 00380 #define BIT_FIFO_SIZE_1024 (0x40) 00381 #define BIT_FIFO_SIZE_2048 (0x80) 00382 #define BIT_FIFO_SIZE_4096 (0xC0) 00383 #define BIT_RESET (0x80) 00384 #define BIT_SLEEP (0x40) 00385 #define BIT_S0_DELAY_EN (0x01) 00386 #define BIT_S2_DELAY_EN (0x04) 00387 #define BITS_SLAVE_LENGTH (0x0F) 00388 #define BIT_SLAVE_BYTE_SW (0x40) 00389 #define BIT_SLAVE_GROUP (0x10) 00390 #define BIT_SLAVE_EN (0x80) 00391 #define BIT_I2C_READ (0x80) 00392 #define BITS_I2C_MASTER_DLY (0x1F) 00393 #define BIT_AUX_IF_EN (0x20) 00394 #define BIT_ACTL (0x80) 00395 #define BIT_LATCH_EN (0x20) 00396 #define BIT_ANY_RD_CLR (0x10) 00397 #define BIT_BYPASS_EN (0x02) 00398 #define BITS_WOM_EN (0xC0) 00399 #define BIT_LPA_CYCLE (0x20) 00400 #define BIT_STBY_XA (0x20) 00401 #define BIT_STBY_YA (0x10) 00402 #define BIT_STBY_ZA (0x08) 00403 #define BIT_STBY_XG (0x04) 00404 #define BIT_STBY_YG (0x02) 00405 #define BIT_STBY_ZG (0x01) 00406 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) 00407 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) 00408 00409 #if defined AK8975_SECONDARY 00410 #define SUPPORTS_AK89xx_HIGH_SENS (0x00) 00411 #define AK89xx_FSR (9830) 00412 #elif defined AK8963_SECONDARY 00413 #define SUPPORTS_AK89xx_HIGH_SENS (0x10) 00414 #define AK89xx_FSR (4915) 00415 #endif 00416 00417 #ifdef AK89xx_SECONDARY 00418 #define AKM_REG_WHOAMI (0x00) 00419 00420 #define AKM_REG_ST1 (0x02) 00421 #define AKM_REG_HXL (0x03) 00422 #define AKM_REG_ST2 (0x09) 00423 00424 #define AKM_REG_CNTL (0x0A) 00425 #define AKM_REG_ASTC (0x0C) 00426 #define AKM_REG_ASAX (0x10) 00427 #define AKM_REG_ASAY (0x11) 00428 #define AKM_REG_ASAZ (0x12) 00429 00430 #define AKM_DATA_READY (0x01) 00431 #define AKM_DATA_OVERRUN (0x02) 00432 #define AKM_OVERFLOW (0x80) 00433 #define AKM_DATA_ERROR (0x40) 00434 00435 #define AKM_BIT_SELF_TEST (0x40) 00436 00437 #define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) 00438 #define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) 00439 #define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) 00440 #define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) 00441 00442 #define AKM_WHOAMI (0x48) 00443 #endif 00444 00445 #if defined MPU6050 00446 //const struct gyro_reg_s reg = { 00447 // .who_am_i = 0x75, 00448 // .rate_div = 0x19, 00449 // .lpf = 0x1A, 00450 // .prod_id = 0x0C, 00451 // .user_ctrl = 0x6A, 00452 // .fifo_en = 0x23, 00453 // .gyro_cfg = 0x1B, 00454 // .accel_cfg = 0x1C, 00455 // .motion_thr = 0x1F, 00456 // .motion_dur = 0x20, 00457 // .fifo_count_h = 0x72, 00458 // .fifo_r_w = 0x74, 00459 // .raw_gyro = 0x43, 00460 // .raw_accel = 0x3B, 00461 // .temp = 0x41, 00462 // .int_enable = 0x38, 00463 // .dmp_int_status = 0x39, 00464 // .int_status = 0x3A, 00465 // .pwr_mgmt_1 = 0x6B, 00466 // .pwr_mgmt_2 = 0x6C, 00467 // .int_pin_cfg = 0x37, 00468 // .mem_r_w = 0x6F, 00469 // .accel_offs = 0x06, 00470 // .i2c_mst = 0x24, 00471 // .bank_sel = 0x6D, 00472 // .mem_start_addr = 0x6E, 00473 // .prgm_start_h = 0x70 00474 //#ifdef AK89xx_SECONDARY 00475 // ,.raw_compass = 0x49, 00476 // .yg_offs_tc = 0x01, 00477 // .s0_addr = 0x25, 00478 // .s0_reg = 0x26, 00479 // .s0_ctrl = 0x27, 00480 // .s1_addr = 0x28, 00481 // .s1_reg = 0x29, 00482 // .s1_ctrl = 0x2A, 00483 // .s4_ctrl = 0x34, 00484 // .s0_do = 0x63, 00485 // .s1_do = 0x64, 00486 // .i2c_delay_ctrl = 0x67 00487 //#endif 00488 //}; 00489 const struct gyro_reg_s reg = { 00490 0x75, //who_am_i 00491 0x19, //rate_div 00492 0x1A, //lpf 00493 0x0C, //prod_id 00494 0x6A, //user_ctrl 00495 0x23, //fifo_en 00496 0x1B, //gyro_cfg 00497 0x1C, //accel_cfg 00498 0x1F, // motion_thr 00499 0x20, // motion_dur 00500 0x72, // fifo_count_h 00501 0x74, // fifo_r_w 00502 0x43, // raw_gyro 00503 0x3B, // raw_accel 00504 0x41, // temp 00505 0x38, // int_enable 00506 0x39, // dmp_int_status 00507 0x3A, // int_status 00508 0x6B, // pwr_mgmt_1 00509 0x6C, // pwr_mgmt_2 00510 0x37, // int_pin_cfg 00511 0x6F, // mem_r_w 00512 0x06, // accel_offs 00513 0x24, // i2c_mst 00514 0x6D, // bank_sel 00515 0x6E, // mem_start_addr 00516 0x70 // prgm_start_h 00517 }; 00518 00519 //const struct hw_s hw = { 00520 // .addr = 0x68, 00521 // .max_fifo = 1024, 00522 // .num_reg = 118, 00523 // .temp_sens = 340, 00524 // .temp_offset = -521, 00525 // .bank_size = 256 00526 //#if defined AK89xx_SECONDARY 00527 // ,.compass_fsr = AK89xx_FSR 00528 //#endif 00529 //}; 00530 const struct hw_s hw={ 00531 0x68, //addr 00532 1024, //max_fifo 00533 118, //num_reg 00534 340, //temp_sens 00535 -521, //temp_offset 00536 256 //bank_size 00537 }; 00538 00539 //const struct test_s test = { 00540 // .gyro_sens = 32768/250, 00541 // .accel_sens = 32768/16, 00542 // .reg_rate_div = 0, /* 1kHz. */ 00543 // .reg_lpf = 1, /* 188Hz. */ 00544 // .reg_gyro_fsr = 0, /* 250dps. */ 00545 // .reg_accel_fsr = 0x18, /* 16g. */ 00546 // .wait_ms = 50, 00547 // .packet_thresh = 5, /* 5% */ 00548 // .min_dps = 10.f, 00549 // .max_dps = 105.f, 00550 // .max_gyro_var = 0.14f, 00551 // .min_g = 0.3f, 00552 // .max_g = 0.95f, 00553 // .max_accel_var = 0.14f 00554 //}; 00555 const struct test_s test={ 00556 32768/250, //gyro_sens 00557 32768/16, // accel_sens 00558 0, // reg_rate_div 00559 1, // reg_lpf 00560 0, // reg_gyro_fsr 00561 0x18, // reg_accel_fsr 00562 50, // wait_ms 00563 5, // packet_thresh 00564 10.0f, // min_dps 00565 105.0f, // max_dps 00566 0.14f, // max_gyro_var 00567 0.3f, // min_g 00568 0.95f, // max_g 00569 0.14f // max_accel_var 00570 }; 00571 00572 //static struct gyro_state_s st = { 00573 // .reg = &reg, 00574 // .hw = &hw, 00575 // .test = &test 00576 //}; 00577 static struct gyro_state_s st={ 00578 &reg, 00579 &hw, 00580 {0}, 00581 &test 00582 }; 00583 00584 00585 #elif defined MPU6500 00586 const struct gyro_reg_s reg = { 00587 .who_am_i = 0x75, 00588 .rate_div = 0x19, 00589 .lpf = 0x1A, 00590 .prod_id = 0x0C, 00591 .user_ctrl = 0x6A, 00592 .fifo_en = 0x23, 00593 .gyro_cfg = 0x1B, 00594 .accel_cfg = 0x1C, 00595 .accel_cfg2 = 0x1D, 00596 .lp_accel_odr = 0x1E, 00597 .motion_thr = 0x1F, 00598 .motion_dur = 0x20, 00599 .fifo_count_h = 0x72, 00600 .fifo_r_w = 0x74, 00601 .raw_gyro = 0x43, 00602 .raw_accel = 0x3B, 00603 .temp = 0x41, 00604 .int_enable = 0x38, 00605 .dmp_int_status = 0x39, 00606 .int_status = 0x3A, 00607 .accel_intel = 0x69, 00608 .pwr_mgmt_1 = 0x6B, 00609 .pwr_mgmt_2 = 0x6C, 00610 .int_pin_cfg = 0x37, 00611 .mem_r_w = 0x6F, 00612 .accel_offs = 0x77, 00613 .i2c_mst = 0x24, 00614 .bank_sel = 0x6D, 00615 .mem_start_addr = 0x6E, 00616 .prgm_start_h = 0x70 00617 #ifdef AK89xx_SECONDARY 00618 ,.raw_compass = 0x49, 00619 .s0_addr = 0x25, 00620 .s0_reg = 0x26, 00621 .s0_ctrl = 0x27, 00622 .s1_addr = 0x28, 00623 .s1_reg = 0x29, 00624 .s1_ctrl = 0x2A, 00625 .s4_ctrl = 0x34, 00626 .s0_do = 0x63, 00627 .s1_do = 0x64, 00628 .i2c_delay_ctrl = 0x67 00629 #endif 00630 }; 00631 const struct hw_s hw = { 00632 .addr = 0x68, 00633 .max_fifo = 1024, 00634 .num_reg = 128, 00635 .temp_sens = 321, 00636 .temp_offset = 0, 00637 .bank_size = 256 00638 #if defined AK89xx_SECONDARY 00639 ,.compass_fsr = AK89xx_FSR 00640 #endif 00641 }; 00642 00643 const struct test_s test = { 00644 .gyro_sens = 32768/250, 00645 .accel_sens = 32768/16, 00646 .reg_rate_div = 0, /* 1kHz. */ 00647 .reg_lpf = 1, /* 188Hz. */ 00648 .reg_gyro_fsr = 0, /* 250dps. */ 00649 .reg_accel_fsr = 0x18, /* 16g. */ 00650 .wait_ms = 50, 00651 .packet_thresh = 5, /* 5% */ 00652 .min_dps = 10.f, 00653 .max_dps = 105.f, 00654 .max_gyro_var = 0.14f, 00655 .min_g = 0.3f, 00656 .max_g = 0.95f, 00657 .max_accel_var = 0.14f 00658 }; 00659 00660 static struct gyro_state_s st = { 00661 .reg = &reg, 00662 .hw = &hw, 00663 .test = &test 00664 }; 00665 #endif 00666 00667 #define MAX_PACKET_LENGTH (12) 00668 00669 #ifdef AK89xx_SECONDARY 00670 static int setup_compass(void); 00671 #define MAX_COMPASS_SAMPLE_RATE (100) 00672 #endif 00673 00681 static int set_int_enable(unsigned char enable) 00682 { 00683 unsigned char tmp; 00684 00685 if (st.chip_cfg.dmp_on) { 00686 if (enable) 00687 tmp = BIT_DMP_INT_EN; 00688 else 00689 tmp = 0x00; 00690 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 00691 return -1; 00692 st.chip_cfg.int_enable = tmp; 00693 } else { 00694 if (!st.chip_cfg.sensors) 00695 return -1; 00696 if (enable && st.chip_cfg.int_enable) 00697 return 0; 00698 if (enable) 00699 tmp = BIT_DATA_RDY_EN; 00700 else 00701 tmp = 0x00; 00702 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 00703 return -1; 00704 st.chip_cfg.int_enable = tmp; 00705 } 00706 return 0; 00707 } 00708 00713 int mpu_reg_dump(void) 00714 { 00715 unsigned char ii; 00716 unsigned char data; 00717 00718 for (ii = 0; ii < st.hw->num_reg; ii++) { 00719 if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) 00720 continue; 00721 if (i2c_read(st.hw->addr, ii, 1, &data)) 00722 return -1; 00723 log_i("%#5x: %#5x\r\n", ii, data); 00724 } 00725 return 0; 00726 } 00727 00735 int mpu_read_reg(unsigned char reg, unsigned char *data) 00736 { 00737 if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) 00738 return -1; 00739 if (reg >= st.hw->num_reg) 00740 return -1; 00741 return i2c_read(st.hw->addr, reg, 1, data); 00742 } 00743 00757 int mpu_init(void) 00758 { 00759 unsigned char data[6], rev; 00760 00761 /* Reset device. */ 00762 data[0] = BIT_RESET; 00763 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 00764 return -1; 00765 delay_ms(100); 00766 00767 /* Wake up chip. */ 00768 data[0] = 0x00; 00769 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 00770 return -1; 00771 00772 #if defined MPU6050 00773 /* Check product revision. */ 00774 if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data)) 00775 return -1; 00776 rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) | 00777 (data[1] & 0x01); 00778 00779 if (rev) { 00780 /* Congrats, these parts are better. */ 00781 if (rev == 1) 00782 st.chip_cfg.accel_half = 1; 00783 else if (rev == 2) 00784 st.chip_cfg.accel_half = 0; 00785 else { 00786 log_e("Unsupported software product rev %d.\n", rev); 00787 return -1; 00788 } 00789 } else { 00790 if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data)) 00791 return -1; 00792 rev = data[0] & 0x0F; 00793 if (!rev) { 00794 log_e("Product ID read as 0 indicates device is either " 00795 "incompatible or an MPU3050.\n"); 00796 return -1; 00797 } else if (rev == 4) { 00798 log_i("Half sensitivity part found.\n"); 00799 st.chip_cfg.accel_half = 1; 00800 } else 00801 st.chip_cfg.accel_half = 0; 00802 } 00803 #elif defined MPU6500 00804 #define MPU6500_MEM_REV_ADDR (0x17) 00805 if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev)) 00806 return -1; 00807 if (rev == 0x1) 00808 st.chip_cfg.accel_half = 0; 00809 else { 00810 log_e("Unsupported software product rev %d.\n", rev); 00811 return -1; 00812 } 00813 00814 /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the 00815 * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. 00816 */ 00817 data[0] = BIT_FIFO_SIZE_1024 | 0x8; 00818 if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) 00819 return -1; 00820 #endif 00821 00822 /* Set to invalid values to ensure no I2C writes are skipped. */ 00823 st.chip_cfg.sensors = 0xFF; 00824 st.chip_cfg.gyro_fsr = 0xFF; 00825 st.chip_cfg.accel_fsr = 0xFF; 00826 st.chip_cfg.lpf = 0xFF; 00827 st.chip_cfg.sample_rate = 0xFFFF; 00828 st.chip_cfg.fifo_enable = 0xFF; 00829 st.chip_cfg.bypass_mode = 0xFF; 00830 #ifdef AK89xx_SECONDARY 00831 st.chip_cfg.compass_sample_rate = 0xFFFF; 00832 #endif 00833 /* mpu_set_sensors always preserves this setting. */ 00834 st.chip_cfg.clk_src = INV_CLK_PLL; 00835 /* Handled in next call to mpu_set_bypass. */ 00836 st.chip_cfg.active_low_int = 1; 00837 st.chip_cfg.latched_int = 0; 00838 st.chip_cfg.int_motion_only = 0; 00839 st.chip_cfg.lp_accel_mode = 0; 00840 memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); 00841 st.chip_cfg.dmp_on = 0; 00842 st.chip_cfg.dmp_loaded = 0; 00843 st.chip_cfg.dmp_sample_rate = 0; 00844 00845 if (mpu_set_gyro_fsr(2000)) 00846 return -1; 00847 if (mpu_set_accel_fsr(2)) 00848 return -1; 00849 if (mpu_set_lpf(42)) 00850 return -1; 00851 if (mpu_set_sample_rate(50)) 00852 return -1; 00853 if (mpu_configure_fifo(0)) 00854 return -1; 00855 00856 // if (int_param) 00857 // reg_int_cb(int_param); 00858 00859 #ifdef AK89xx_SECONDARY 00860 setup_compass(); 00861 if (mpu_set_compass_sample_rate(10)) 00862 return -1; 00863 #else 00864 /* Already disabled by setup_compass. */ 00865 if (mpu_set_bypass(0)) 00866 return -1; 00867 #endif 00868 00869 mpu_set_sensors(0); 00870 return 0; 00871 } 00872 00888 int mpu_lp_accel_mode(unsigned char rate) 00889 { 00890 unsigned char tmp[2]; 00891 00892 if (rate > 40) 00893 return -1; 00894 00895 if (!rate) { 00896 mpu_set_int_latched(0); 00897 tmp[0] = 0; 00898 tmp[1] = BIT_STBY_XYZG; 00899 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 00900 return -1; 00901 st.chip_cfg.lp_accel_mode = 0; 00902 return 0; 00903 } 00904 /* For LP accel, we automatically configure the hardware to produce latched 00905 * interrupts. In LP accel mode, the hardware cycles into sleep mode before 00906 * it gets a chance to deassert the interrupt pin; therefore, we shift this 00907 * responsibility over to the MCU. 00908 * 00909 * Any register read will clear the interrupt. 00910 */ 00911 mpu_set_int_latched(1); 00912 #if defined MPU6050 00913 tmp[0] = BIT_LPA_CYCLE; 00914 if (rate == 1) { 00915 tmp[1] = INV_LPA_1_25HZ; 00916 mpu_set_lpf(5); 00917 } else if (rate <= 5) { 00918 tmp[1] = INV_LPA_5HZ; 00919 mpu_set_lpf(5); 00920 } else if (rate <= 20) { 00921 tmp[1] = INV_LPA_20HZ; 00922 mpu_set_lpf(10); 00923 } else { 00924 tmp[1] = INV_LPA_40HZ; 00925 mpu_set_lpf(20); 00926 } 00927 tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; 00928 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 00929 return -1; 00930 #elif defined MPU6500 00931 /* Set wake frequency. */ 00932 if (rate == 1) 00933 tmp[0] = INV_LPA_1_25HZ; 00934 else if (rate == 2) 00935 tmp[0] = INV_LPA_2_5HZ; 00936 else if (rate <= 5) 00937 tmp[0] = INV_LPA_5HZ; 00938 else if (rate <= 10) 00939 tmp[0] = INV_LPA_10HZ; 00940 else if (rate <= 20) 00941 tmp[0] = INV_LPA_20HZ; 00942 else if (rate <= 40) 00943 tmp[0] = INV_LPA_40HZ; 00944 else if (rate <= 80) 00945 tmp[0] = INV_LPA_80HZ; 00946 else if (rate <= 160) 00947 tmp[0] = INV_LPA_160HZ; 00948 else if (rate <= 320) 00949 tmp[0] = INV_LPA_320HZ; 00950 else 00951 tmp[0] = INV_LPA_640HZ; 00952 if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) 00953 return -1; 00954 tmp[0] = BIT_LPA_CYCLE; 00955 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) 00956 return -1; 00957 #endif 00958 st.chip_cfg.sensors = INV_XYZ_ACCEL; 00959 st.chip_cfg.clk_src = 0; 00960 st.chip_cfg.lp_accel_mode = 1; 00961 mpu_configure_fifo(0); 00962 00963 return 0; 00964 } 00965 00972 int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 00973 { 00974 unsigned char tmp[6]; 00975 00976 if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) 00977 return -1; 00978 00979 if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) 00980 return -1; 00981 data[0] = (tmp[0] << 8) | tmp[1]; 00982 data[1] = (tmp[2] << 8) | tmp[3]; 00983 data[2] = (tmp[4] << 8) | tmp[5]; 00984 if (timestamp) 00985 get_ms(timestamp); 00986 return 0; 00987 } 00988 00995 int mpu_get_accel_reg(short *data, unsigned long *timestamp) 00996 { 00997 unsigned char tmp[6]; 00998 00999 if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) 01000 return -1; 01001 01002 if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) 01003 return -1; 01004 data[0] = (tmp[0] << 8) | tmp[1]; 01005 data[1] = (tmp[2] << 8) | tmp[3]; 01006 data[2] = (tmp[4] << 8) | tmp[5]; 01007 if (timestamp) 01008 get_ms(timestamp); 01009 return 0; 01010 } 01011 01018 int mpu_get_temperature(long *data, unsigned long *timestamp) 01019 { 01020 unsigned char tmp[2]; 01021 short raw; 01022 01023 if (!(st.chip_cfg.sensors)) 01024 return -1; 01025 01026 if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp)) 01027 return -1; 01028 raw = (tmp[0] << 8) | tmp[1]; 01029 if (timestamp) 01030 get_ms(timestamp); 01031 01032 data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); 01033 return 0; 01034 } 01035 01043 int mpu_set_accel_bias(const long *accel_bias) 01044 { 01045 unsigned char data[6]; 01046 short accel_hw[3]; 01047 short got_accel[3]; 01048 short fg[3]; 01049 01050 if (!accel_bias) 01051 return -1; 01052 if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2]) 01053 return 0; 01054 01055 if (i2c_read(st.hw->addr, 3, 3, data)) 01056 return -1; 01057 fg[0] = ((data[0] >> 4) + 8) & 0xf; 01058 fg[1] = ((data[1] >> 4) + 8) & 0xf; 01059 fg[2] = ((data[2] >> 4) + 8) & 0xf; 01060 01061 accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0])); 01062 accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1])); 01063 accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2])); 01064 01065 if (i2c_read(st.hw->addr, 0x06, 6, data)) 01066 return -1; 01067 01068 got_accel[0] = ((short)data[0] << 8) | data[1]; 01069 got_accel[1] = ((short)data[2] << 8) | data[3]; 01070 got_accel[2] = ((short)data[4] << 8) | data[5]; 01071 01072 accel_hw[0] += got_accel[0]; 01073 accel_hw[1] += got_accel[1]; 01074 accel_hw[2] += got_accel[2]; 01075 01076 data[0] = (accel_hw[0] >> 8) & 0xff; 01077 data[1] = (accel_hw[0]) & 0xff; 01078 data[2] = (accel_hw[1] >> 8) & 0xff; 01079 data[3] = (accel_hw[1]) & 0xff; 01080 data[4] = (accel_hw[2] >> 8) & 0xff; 01081 data[5] = (accel_hw[2]) & 0xff; 01082 01083 if (i2c_write(st.hw->addr, 0x06, 6, data)) 01084 return -1; 01085 return 0; 01086 } 01087 01092 int mpu_reset_fifo(void) 01093 { 01094 unsigned char data; 01095 01096 if (!(st.chip_cfg.sensors)) 01097 return -1; 01098 01099 data = 0; 01100 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01101 return -1; 01102 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01103 return -1; 01104 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01105 return -1; 01106 01107 if (st.chip_cfg.dmp_on) { 01108 data = BIT_FIFO_RST | BIT_DMP_RST; 01109 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01110 return -1; 01111 delay_ms(50); 01112 data = BIT_DMP_EN | BIT_FIFO_EN; 01113 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 01114 data |= BIT_AUX_IF_EN; 01115 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01116 return -1; 01117 if (st.chip_cfg.int_enable) 01118 data = BIT_DMP_INT_EN; 01119 else 01120 data = 0; 01121 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01122 return -1; 01123 data = 0; 01124 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01125 return -1; 01126 } else { 01127 data = BIT_FIFO_RST; 01128 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01129 return -1; 01130 if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 01131 data = BIT_FIFO_EN; 01132 else 01133 data = BIT_FIFO_EN | BIT_AUX_IF_EN; 01134 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01135 return -1; 01136 delay_ms(50); 01137 if (st.chip_cfg.int_enable) 01138 data = BIT_DATA_RDY_EN; 01139 else 01140 data = 0; 01141 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01142 return -1; 01143 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) 01144 return -1; 01145 } 01146 return 0; 01147 } 01148 01154 int mpu_get_gyro_fsr(unsigned short *fsr) 01155 { 01156 switch (st.chip_cfg.gyro_fsr) { 01157 case INV_FSR_250DPS: 01158 fsr[0] = 250; 01159 break; 01160 case INV_FSR_500DPS: 01161 fsr[0] = 500; 01162 break; 01163 case INV_FSR_1000DPS: 01164 fsr[0] = 1000; 01165 break; 01166 case INV_FSR_2000DPS: 01167 fsr[0] = 2000; 01168 break; 01169 default: 01170 fsr[0] = 0; 01171 break; 01172 } 01173 return 0; 01174 } 01175 01181 int mpu_set_gyro_fsr(unsigned short fsr) 01182 { 01183 unsigned char data; 01184 01185 if (!(st.chip_cfg.sensors)) 01186 return -1; 01187 01188 switch (fsr) { 01189 case 250: 01190 data = INV_FSR_250DPS << 3; 01191 break; 01192 case 500: 01193 data = INV_FSR_500DPS << 3; 01194 break; 01195 case 1000: 01196 data = INV_FSR_1000DPS << 3; 01197 break; 01198 case 2000: 01199 data = INV_FSR_2000DPS << 3; 01200 break; 01201 default: 01202 return -1; 01203 } 01204 01205 if (st.chip_cfg.gyro_fsr == (data >> 3)) 01206 return 0; 01207 if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) 01208 return -1; 01209 st.chip_cfg.gyro_fsr = data >> 3; 01210 return 0; 01211 } 01212 01218 int mpu_get_accel_fsr(unsigned char *fsr) 01219 { 01220 switch (st.chip_cfg.accel_fsr) { 01221 case INV_FSR_2G: 01222 fsr[0] = 2; 01223 break; 01224 case INV_FSR_4G: 01225 fsr[0] = 4; 01226 break; 01227 case INV_FSR_8G: 01228 fsr[0] = 8; 01229 break; 01230 case INV_FSR_16G: 01231 fsr[0] = 16; 01232 break; 01233 default: 01234 return -1; 01235 } 01236 if (st.chip_cfg.accel_half) 01237 fsr[0] <<= 1; 01238 return 0; 01239 } 01240 01246 int mpu_set_accel_fsr(unsigned char fsr) 01247 { 01248 unsigned char data; 01249 01250 if (!(st.chip_cfg.sensors)) 01251 return -1; 01252 01253 switch (fsr) { 01254 case 2: 01255 data = INV_FSR_2G << 3; 01256 break; 01257 case 4: 01258 data = INV_FSR_4G << 3; 01259 break; 01260 case 8: 01261 data = INV_FSR_8G << 3; 01262 break; 01263 case 16: 01264 data = INV_FSR_16G << 3; 01265 break; 01266 default: 01267 return -1; 01268 } 01269 01270 if (st.chip_cfg.accel_fsr == (data >> 3)) 01271 return 0; 01272 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) 01273 return -1; 01274 st.chip_cfg.accel_fsr = data >> 3; 01275 return 0; 01276 } 01277 01283 int mpu_get_lpf(unsigned short *lpf) 01284 { 01285 switch (st.chip_cfg.lpf) { 01286 case INV_FILTER_188HZ: 01287 lpf[0] = 188; 01288 break; 01289 case INV_FILTER_98HZ: 01290 lpf[0] = 98; 01291 break; 01292 case INV_FILTER_42HZ: 01293 lpf[0] = 42; 01294 break; 01295 case INV_FILTER_20HZ: 01296 lpf[0] = 20; 01297 break; 01298 case INV_FILTER_10HZ: 01299 lpf[0] = 10; 01300 break; 01301 case INV_FILTER_5HZ: 01302 lpf[0] = 5; 01303 break; 01304 case INV_FILTER_256HZ_NOLPF2: 01305 case INV_FILTER_2100HZ_NOLPF: 01306 default: 01307 lpf[0] = 0; 01308 break; 01309 } 01310 return 0; 01311 } 01312 01319 int mpu_set_lpf(unsigned short lpf) 01320 { 01321 unsigned char data; 01322 01323 if (!(st.chip_cfg.sensors)) 01324 return -1; 01325 01326 if (lpf >= 188) 01327 data = INV_FILTER_188HZ; 01328 else if (lpf >= 98) 01329 data = INV_FILTER_98HZ; 01330 else if (lpf >= 42) 01331 data = INV_FILTER_42HZ; 01332 else if (lpf >= 20) 01333 data = INV_FILTER_20HZ; 01334 else if (lpf >= 10) 01335 data = INV_FILTER_10HZ; 01336 else 01337 data = INV_FILTER_5HZ; 01338 01339 if (st.chip_cfg.lpf == data) 01340 return 0; 01341 if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data)) 01342 return -1; 01343 st.chip_cfg.lpf = data; 01344 return 0; 01345 } 01346 01352 int mpu_get_sample_rate(unsigned short *rate) 01353 { 01354 if (st.chip_cfg.dmp_on) 01355 return -1; 01356 else 01357 rate[0] = st.chip_cfg.sample_rate; 01358 return 0; 01359 } 01360 01367 int mpu_set_sample_rate(unsigned short rate) 01368 { 01369 unsigned char data; 01370 01371 if (!(st.chip_cfg.sensors)) 01372 return -1; 01373 01374 if (st.chip_cfg.dmp_on) 01375 return -1; 01376 else { 01377 if (st.chip_cfg.lp_accel_mode) { 01378 if (rate && (rate <= 40)) { 01379 /* Just stay in low-power accel mode. */ 01380 mpu_lp_accel_mode(rate); 01381 return 0; 01382 } 01383 /* Requested rate exceeds the allowed frequencies in LP accel mode, 01384 * switch back to full-power mode. 01385 */ 01386 mpu_lp_accel_mode(0); 01387 } 01388 if (rate < 4) 01389 rate = 4; 01390 else if (rate > 1000) 01391 rate = 1000; 01392 01393 data = 1000 / rate - 1; 01394 if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data)) 01395 return -1; 01396 01397 st.chip_cfg.sample_rate = 1000 / (1 + data); 01398 01399 #ifdef AK89xx_SECONDARY 01400 mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); 01401 #endif 01402 01403 /* Automatically set LPF to 1/2 sampling rate. */ 01404 mpu_set_lpf(st.chip_cfg.sample_rate >> 1); 01405 return 0; 01406 } 01407 } 01408 01414 int mpu_get_compass_sample_rate(unsigned short *rate) 01415 { 01416 #ifdef AK89xx_SECONDARY 01417 rate[0] = st.chip_cfg.compass_sample_rate; 01418 return 0; 01419 #else 01420 rate[0] = 0; 01421 return -1; 01422 #endif 01423 } 01424 01436 int mpu_set_compass_sample_rate(unsigned short rate) 01437 { 01438 #ifdef AK89xx_SECONDARY 01439 unsigned char div; 01440 if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) 01441 return -1; 01442 01443 div = st.chip_cfg.sample_rate / rate - 1; 01444 if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) 01445 return -1; 01446 st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); 01447 return 0; 01448 #else 01449 return -1; 01450 #endif 01451 } 01452 01458 int mpu_get_gyro_sens(float *sens) 01459 { 01460 switch (st.chip_cfg.gyro_fsr) { 01461 case INV_FSR_250DPS: 01462 sens[0] = 131.f; 01463 break; 01464 case INV_FSR_500DPS: 01465 sens[0] = 65.5f; 01466 break; 01467 case INV_FSR_1000DPS: 01468 sens[0] = 32.8f; 01469 break; 01470 case INV_FSR_2000DPS: 01471 sens[0] = 16.4f; 01472 break; 01473 default: 01474 return -1; 01475 } 01476 return 0; 01477 } 01478 01484 int mpu_get_accel_sens(unsigned short *sens) 01485 { 01486 switch (st.chip_cfg.accel_fsr) { 01487 case INV_FSR_2G: 01488 sens[0] = 16384; 01489 break; 01490 case INV_FSR_4G: 01491 sens[0] = 8092; 01492 break; 01493 case INV_FSR_8G: 01494 sens[0] = 4096; 01495 break; 01496 case INV_FSR_16G: 01497 sens[0] = 2048; 01498 break; 01499 default: 01500 return -1; 01501 } 01502 if (st.chip_cfg.accel_half) 01503 sens[0] >>= 1; 01504 return 0; 01505 } 01506 01516 int mpu_get_fifo_config(unsigned char *sensors) 01517 { 01518 sensors[0] = st.chip_cfg.fifo_enable; 01519 return 0; 01520 } 01521 01531 int mpu_configure_fifo(unsigned char sensors) 01532 { 01533 unsigned char prev; 01534 int result = 0; 01535 01536 /* Compass data isn't going into the FIFO. Stop trying. */ 01537 sensors &= ~INV_XYZ_COMPASS; 01538 01539 if (st.chip_cfg.dmp_on) 01540 return 0; 01541 else { 01542 if (!(st.chip_cfg.sensors)) 01543 return -1; 01544 prev = st.chip_cfg.fifo_enable; 01545 st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; 01546 if (st.chip_cfg.fifo_enable != sensors) 01547 /* You're not getting what you asked for. Some sensors are 01548 * asleep. 01549 */ 01550 result = -1; 01551 else 01552 result = 0; 01553 if (sensors || st.chip_cfg.lp_accel_mode) 01554 set_int_enable(1); 01555 else 01556 set_int_enable(0); 01557 if (sensors) { 01558 if (mpu_reset_fifo()) { 01559 st.chip_cfg.fifo_enable = prev; 01560 return -1; 01561 } 01562 } 01563 } 01564 01565 return result; 01566 } 01567 01573 int mpu_get_power_state(unsigned char *power_on) 01574 { 01575 if (st.chip_cfg.sensors) 01576 power_on[0] = 1; 01577 else 01578 power_on[0] = 0; 01579 return 0; 01580 } 01581 01592 int mpu_set_sensors(unsigned char sensors) 01593 { 01594 unsigned char data; 01595 #ifdef AK89xx_SECONDARY 01596 unsigned char user_ctrl; 01597 #endif 01598 01599 if (sensors & INV_XYZ_GYRO) 01600 data = INV_CLK_PLL; 01601 else if (sensors) 01602 data = 0; 01603 else 01604 data = BIT_SLEEP; 01605 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { 01606 st.chip_cfg.sensors = 0; 01607 return -1; 01608 } 01609 st.chip_cfg.clk_src = data & ~BIT_SLEEP; 01610 01611 data = 0; 01612 if (!(sensors & INV_X_GYRO)) 01613 data |= BIT_STBY_XG; 01614 if (!(sensors & INV_Y_GYRO)) 01615 data |= BIT_STBY_YG; 01616 if (!(sensors & INV_Z_GYRO)) 01617 data |= BIT_STBY_ZG; 01618 if (!(sensors & INV_XYZ_ACCEL)) 01619 data |= BIT_STBY_XYZA; 01620 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { 01621 st.chip_cfg.sensors = 0; 01622 return -1; 01623 } 01624 01625 if (sensors && (sensors != INV_XYZ_ACCEL)) 01626 /* Latched interrupts only used in LP accel mode. */ 01627 mpu_set_int_latched(0); 01628 01629 #ifdef AK89xx_SECONDARY 01630 #ifdef AK89xx_BYPASS 01631 if (sensors & INV_XYZ_COMPASS) 01632 mpu_set_bypass(1); 01633 else 01634 mpu_set_bypass(0); 01635 #else 01636 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01637 return -1; 01638 /* Handle AKM power management. */ 01639 if (sensors & INV_XYZ_COMPASS) { 01640 data = AKM_SINGLE_MEASUREMENT; 01641 user_ctrl |= BIT_AUX_IF_EN; 01642 } else { 01643 data = AKM_POWER_DOWN; 01644 user_ctrl &= ~BIT_AUX_IF_EN; 01645 } 01646 if (st.chip_cfg.dmp_on) 01647 user_ctrl |= BIT_DMP_EN; 01648 else 01649 user_ctrl &= ~BIT_DMP_EN; 01650 if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) 01651 return -1; 01652 /* Enable/disable I2C master mode. */ 01653 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01654 return -1; 01655 #endif 01656 #endif 01657 01658 st.chip_cfg.sensors = sensors; 01659 st.chip_cfg.lp_accel_mode = 0; 01660 delay_ms(50); 01661 return 0; 01662 } 01663 01669 int mpu_get_int_status(short *status) 01670 { 01671 unsigned char tmp[2]; 01672 if (!st.chip_cfg.sensors) 01673 return -1; 01674 if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) 01675 return -1; 01676 status[0] = (tmp[0] << 8) | tmp[1]; 01677 return 0; 01678 } 01679 01698 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, 01699 unsigned char *sensors, unsigned char *more) 01700 { 01701 /* Assumes maximum packet size is gyro (6) + accel (6). */ 01702 unsigned char data[MAX_PACKET_LENGTH]; 01703 unsigned char packet_size = 0; 01704 unsigned short fifo_count, index = 0; 01705 01706 if (st.chip_cfg.dmp_on) 01707 return -1; 01708 01709 sensors[0] = 0; 01710 if (!st.chip_cfg.sensors) 01711 return -1; 01712 if (!st.chip_cfg.fifo_enable) 01713 return -1; 01714 01715 if (st.chip_cfg.fifo_enable & INV_X_GYRO) 01716 packet_size += 2; 01717 if (st.chip_cfg.fifo_enable & INV_Y_GYRO) 01718 packet_size += 2; 01719 if (st.chip_cfg.fifo_enable & INV_Z_GYRO) 01720 packet_size += 2; 01721 if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) 01722 packet_size += 6; 01723 01724 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 01725 return -1; 01726 fifo_count = (data[0] << 8) | data[1]; 01727 if (fifo_count < packet_size) 01728 return 0; 01729 // log_i("FIFO count: %hd\n", fifo_count); 01730 if (fifo_count > (st.hw->max_fifo >> 1)) { 01731 /* FIFO is 50% full, better check overflow bit. */ 01732 if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) 01733 return -1; 01734 if (data[0] & BIT_FIFO_OVERFLOW) { 01735 mpu_reset_fifo(); 01736 return -2; 01737 } 01738 } 01739 get_ms((unsigned long*)timestamp); 01740 01741 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) 01742 return -1; 01743 more[0] = fifo_count / packet_size - 1; 01744 sensors[0] = 0; 01745 01746 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { 01747 accel[0] = (data[index+0] << 8) | data[index+1]; 01748 accel[1] = (data[index+2] << 8) | data[index+3]; 01749 accel[2] = (data[index+4] << 8) | data[index+5]; 01750 sensors[0] |= INV_XYZ_ACCEL; 01751 index += 6; 01752 } 01753 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { 01754 gyro[0] = (data[index+0] << 8) | data[index+1]; 01755 sensors[0] |= INV_X_GYRO; 01756 index += 2; 01757 } 01758 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { 01759 gyro[1] = (data[index+0] << 8) | data[index+1]; 01760 sensors[0] |= INV_Y_GYRO; 01761 index += 2; 01762 } 01763 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { 01764 gyro[2] = (data[index+0] << 8) | data[index+1]; 01765 sensors[0] |= INV_Z_GYRO; 01766 index += 2; 01767 } 01768 01769 return 0; 01770 } 01771 01779 int mpu_read_fifo_stream(unsigned short length, unsigned char *data, 01780 unsigned char *more) 01781 { 01782 unsigned char tmp[2]; 01783 unsigned short fifo_count; 01784 if (!st.chip_cfg.dmp_on) 01785 return -1; 01786 if (!st.chip_cfg.sensors) 01787 return -1; 01788 01789 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) 01790 return -1; 01791 fifo_count = (tmp[0] << 8) | tmp[1]; 01792 if (fifo_count < length) { 01793 more[0] = 0; 01794 return -1; 01795 } 01796 if (fifo_count > (st.hw->max_fifo >> 1)) { 01797 /* FIFO is 50% full, better check overflow bit. */ 01798 if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp)) 01799 return -1; 01800 if (tmp[0] & BIT_FIFO_OVERFLOW) { 01801 mpu_reset_fifo(); 01802 return -2; 01803 } 01804 } 01805 01806 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data)) 01807 return -1; 01808 more[0] = fifo_count / length - 1; 01809 return 0; 01810 } 01811 01817 int mpu_set_bypass(unsigned char bypass_on) 01818 { 01819 unsigned char tmp; 01820 01821 if (st.chip_cfg.bypass_mode == bypass_on) 01822 return 0; 01823 01824 if (bypass_on) { 01825 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01826 return -1; 01827 tmp &= ~BIT_AUX_IF_EN; 01828 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01829 return -1; 01830 delay_ms(3); 01831 tmp = BIT_BYPASS_EN; 01832 if (st.chip_cfg.active_low_int) 01833 tmp |= BIT_ACTL; 01834 if (st.chip_cfg.latched_int) 01835 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 01836 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01837 return -1; 01838 } else { 01839 /* Enable I2C master mode if compass is being used. */ 01840 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01841 return -1; 01842 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 01843 tmp |= BIT_AUX_IF_EN; 01844 else 01845 tmp &= ~BIT_AUX_IF_EN; 01846 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 01847 return -1; 01848 delay_ms(3); 01849 if (st.chip_cfg.active_low_int) 01850 tmp = BIT_ACTL; 01851 else 01852 tmp = 0; 01853 if (st.chip_cfg.latched_int) 01854 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 01855 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01856 return -1; 01857 } 01858 st.chip_cfg.bypass_mode = bypass_on; 01859 return 0; 01860 } 01861 01867 int mpu_set_int_level(unsigned char active_low) 01868 { 01869 st.chip_cfg.active_low_int = active_low; 01870 return 0; 01871 } 01872 01879 int mpu_set_int_latched(unsigned char enable) 01880 { 01881 unsigned char tmp; 01882 if (st.chip_cfg.latched_int == enable) 01883 return 0; 01884 01885 if (enable) 01886 tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; 01887 else 01888 tmp = 0; 01889 if (st.chip_cfg.bypass_mode) 01890 tmp |= BIT_BYPASS_EN; 01891 if (st.chip_cfg.active_low_int) 01892 tmp |= BIT_ACTL; 01893 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 01894 return -1; 01895 st.chip_cfg.latched_int = enable; 01896 return 0; 01897 } 01898 01899 #ifdef MPU6050 01900 static int get_accel_prod_shift(float *st_shift) 01901 { 01902 unsigned char tmp[4], shift_code[3], ii; 01903 01904 if (i2c_read(st.hw->addr, 0x0D, 4, tmp)) 01905 return 0x07; 01906 01907 shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); 01908 shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); 01909 shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); 01910 for (ii = 0; ii < 3; ii++) { 01911 if (!shift_code[ii]) { 01912 st_shift[ii] = 0.f; 01913 continue; 01914 } 01915 /* Equivalent to.. 01916 * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) 01917 */ 01918 st_shift[ii] = 0.34f; 01919 while (--shift_code[ii]) 01920 st_shift[ii] *= 1.034f; 01921 } 01922 return 0; 01923 } 01924 01925 static int accel_self_test(long *bias_regular, long *bias_st) 01926 { 01927 int jj, result = 0; 01928 float st_shift[3], st_shift_cust, st_shift_var; 01929 01930 get_accel_prod_shift(st_shift); 01931 for(jj = 0; jj < 3; jj++) { 01932 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 01933 if (st_shift[jj]) { 01934 st_shift_var = st_shift_cust / st_shift[jj] - 1.f; 01935 if (fabs(st_shift_var) > test.max_accel_var) 01936 result |= 1 << jj; 01937 } else if ((st_shift_cust < test.min_g) || 01938 (st_shift_cust > test.max_g)) 01939 result |= 1 << jj; 01940 } 01941 01942 return result; 01943 } 01944 01945 static int gyro_self_test(long *bias_regular, long *bias_st) 01946 { 01947 int jj, result = 0; 01948 unsigned char tmp[3]; 01949 float st_shift, st_shift_cust, st_shift_var; 01950 01951 if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) 01952 return 0x07; 01953 01954 tmp[0] &= 0x1F; 01955 tmp[1] &= 0x1F; 01956 tmp[2] &= 0x1F; 01957 01958 for (jj = 0; jj < 3; jj++) { 01959 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 01960 if (tmp[jj]) { 01961 st_shift = 3275.f / test.gyro_sens; 01962 while (--tmp[jj]) 01963 st_shift *= 1.046f; 01964 st_shift_var = st_shift_cust / st_shift - 1.f; 01965 if (fabs(st_shift_var) > test.max_gyro_var) 01966 result |= 1 << jj; 01967 } else if ((st_shift_cust < test.min_dps) || 01968 (st_shift_cust > test.max_dps)) 01969 result |= 1 << jj; 01970 } 01971 return result; 01972 } 01973 01974 #ifdef AK89xx_SECONDARY 01975 static int compass_self_test(void) 01976 { 01977 unsigned char tmp[6]; 01978 unsigned char tries = 10; 01979 int result = 0x07; 01980 short data; 01981 01982 mpu_set_bypass(1); 01983 01984 tmp[0] = AKM_POWER_DOWN; 01985 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 01986 return 0x07; 01987 tmp[0] = AKM_BIT_SELF_TEST; 01988 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) 01989 goto AKM_restore; 01990 tmp[0] = AKM_MODE_SELF_TEST; 01991 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 01992 goto AKM_restore; 01993 01994 do { 01995 delay_ms(10); 01996 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) 01997 goto AKM_restore; 01998 if (tmp[0] & AKM_DATA_READY) 01999 break; 02000 } while (tries--); 02001 if (!(tmp[0] & AKM_DATA_READY)) 02002 goto AKM_restore; 02003 02004 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) 02005 goto AKM_restore; 02006 02007 result = 0; 02008 data = (short)(tmp[1] << 8) | tmp[0]; 02009 if ((data > 100) || (data < -100)) 02010 result |= 0x01; 02011 data = (short)(tmp[3] << 8) | tmp[2]; 02012 if ((data > 100) || (data < -100)) 02013 result |= 0x02; 02014 data = (short)(tmp[5] << 8) | tmp[4]; 02015 if ((data > -300) || (data < -1000)) 02016 result |= 0x04; 02017 02018 AKM_restore: 02019 tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; 02020 i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); 02021 tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; 02022 i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); 02023 mpu_set_bypass(0); 02024 return result; 02025 } 02026 #endif 02027 #endif 02028 02029 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 02030 { 02031 unsigned char data[MAX_PACKET_LENGTH]; 02032 unsigned char packet_count, ii; 02033 unsigned short fifo_count; 02034 02035 data[0] = 0x01; 02036 data[1] = 0; 02037 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 02038 return -1; 02039 delay_ms(200); 02040 data[0] = 0; 02041 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 02042 return -1; 02043 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02044 return -1; 02045 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 02046 return -1; 02047 if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02048 return -1; 02049 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02050 return -1; 02051 data[0] = BIT_FIFO_RST | BIT_DMP_RST; 02052 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02053 return -1; 02054 delay_ms(15); 02055 data[0] = st.test->reg_lpf; 02056 if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) 02057 return -1; 02058 data[0] = st.test->reg_rate_div; 02059 if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) 02060 return -1; 02061 if (hw_test) 02062 data[0] = st.test->reg_gyro_fsr | 0xE0; 02063 else 02064 data[0] = st.test->reg_gyro_fsr; 02065 if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) 02066 return -1; 02067 02068 if (hw_test) 02069 data[0] = st.test->reg_accel_fsr | 0xE0; 02070 else 02071 data[0] = test.reg_accel_fsr; 02072 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 02073 return -1; 02074 if (hw_test) 02075 delay_ms(200); 02076 02077 /* Fill FIFO for test.wait_ms milliseconds. */ 02078 data[0] = BIT_FIFO_EN; 02079 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02080 return -1; 02081 02082 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; 02083 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02084 return -1; 02085 delay_ms(test.wait_ms); 02086 data[0] = 0; 02087 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02088 return -1; 02089 02090 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 02091 return -1; 02092 02093 fifo_count = (data[0] << 8) | data[1]; 02094 packet_count = fifo_count / MAX_PACKET_LENGTH; 02095 gyro[0] = gyro[1] = gyro[2] = 0; 02096 accel[0] = accel[1] = accel[2] = 0; 02097 02098 for (ii = 0; ii < packet_count; ii++) { 02099 short accel_cur[3], gyro_cur[3]; 02100 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) 02101 return -1; 02102 accel_cur[0] = ((short)data[0] << 8) | data[1]; 02103 accel_cur[1] = ((short)data[2] << 8) | data[3]; 02104 accel_cur[2] = ((short)data[4] << 8) | data[5]; 02105 accel[0] += (long)accel_cur[0]; 02106 accel[1] += (long)accel_cur[1]; 02107 accel[2] += (long)accel_cur[2]; 02108 gyro_cur[0] = (((short)data[6] << 8) | data[7]); 02109 gyro_cur[1] = (((short)data[8] << 8) | data[9]); 02110 gyro_cur[2] = (((short)data[10] << 8) | data[11]); 02111 gyro[0] += (long)gyro_cur[0]; 02112 gyro[1] += (long)gyro_cur[1]; 02113 gyro[2] += (long)gyro_cur[2]; 02114 } 02115 #ifdef EMPL_NO_64BIT 02116 gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); 02117 gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); 02118 gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); 02119 if (has_accel) { 02120 accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / 02121 packet_count); 02122 accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / 02123 packet_count); 02124 accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / 02125 packet_count); 02126 /* Don't remove gravity! */ 02127 accel[2] -= 65536L; 02128 } 02129 #else 02130 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); 02131 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); 02132 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); 02133 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / 02134 packet_count); 02135 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / 02136 packet_count); 02137 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / 02138 packet_count); 02139 /* Don't remove gravity! */ 02140 if (accel[2] > 0L) 02141 accel[2] -= 65536L; 02142 else 02143 accel[2] += 65536L; 02144 #endif 02145 02146 return 0; 02147 } 02148 02169 int mpu_run_self_test(long *gyro, long *accel) 02170 { 02171 #ifdef MPU6050 02172 const unsigned char tries = 2; 02173 long gyro_st[3], accel_st[3]; 02174 unsigned char accel_result, gyro_result; 02175 #ifdef AK89xx_SECONDARY 02176 unsigned char compass_result; 02177 #endif 02178 int ii; 02179 #endif 02180 int result; 02181 unsigned char accel_fsr, fifo_sensors, sensors_on; 02182 unsigned short gyro_fsr, sample_rate, lpf; 02183 unsigned char dmp_was_on; 02184 02185 if (st.chip_cfg.dmp_on) { 02186 mpu_set_dmp_state(0); 02187 dmp_was_on = 1; 02188 } else 02189 dmp_was_on = 0; 02190 02191 /* Get initial settings. */ 02192 mpu_get_gyro_fsr(&gyro_fsr); 02193 mpu_get_accel_fsr(&accel_fsr); 02194 mpu_get_lpf(&lpf); 02195 mpu_get_sample_rate(&sample_rate); 02196 sensors_on = st.chip_cfg.sensors; 02197 mpu_get_fifo_config(&fifo_sensors); 02198 02199 /* For older chips, the self-test will be different. */ 02200 #if defined MPU6050 02201 for (ii = 0; ii < tries; ii++) 02202 if (!get_st_biases(gyro, accel, 0)) 02203 break; 02204 if (ii == tries) { 02205 /* If we reach this point, we most likely encountered an I2C error. 02206 * We'll just report an error for all three sensors. 02207 */ 02208 result = 0; 02209 goto restore; 02210 } 02211 for (ii = 0; ii < tries; ii++) 02212 if (!get_st_biases(gyro_st, accel_st, 1)) 02213 break; 02214 if (ii == tries) { 02215 /* Again, probably an I2C error. */ 02216 result = 0; 02217 goto restore; 02218 } 02219 accel_result = accel_self_test(accel, accel_st); 02220 gyro_result = gyro_self_test(gyro, gyro_st); 02221 02222 result = 0; 02223 if (!gyro_result) 02224 result |= 0x01; 02225 if (!accel_result) 02226 result |= 0x02; 02227 02228 #ifdef AK89xx_SECONDARY 02229 compass_result = compass_self_test(); 02230 if (!compass_result) 02231 result |= 0x04; 02232 #endif 02233 restore: 02234 #elif defined MPU6500 02235 /* For now, this function will return a "pass" result for all three sensors 02236 * for compatibility with current test applications. 02237 */ 02238 get_st_biases(gyro, accel, 0); 02239 result = 0x7; 02240 #endif 02241 /* Set to invalid values to ensure no I2C writes are skipped. */ 02242 st.chip_cfg.gyro_fsr = 0xFF; 02243 st.chip_cfg.accel_fsr = 0xFF; 02244 st.chip_cfg.lpf = 0xFF; 02245 st.chip_cfg.sample_rate = 0xFFFF; 02246 st.chip_cfg.sensors = 0xFF; 02247 st.chip_cfg.fifo_enable = 0xFF; 02248 st.chip_cfg.clk_src = INV_CLK_PLL; 02249 mpu_set_gyro_fsr(gyro_fsr); 02250 mpu_set_accel_fsr(accel_fsr); 02251 mpu_set_lpf(lpf); 02252 mpu_set_sample_rate(sample_rate); 02253 mpu_set_sensors(sensors_on); 02254 mpu_configure_fifo(fifo_sensors); 02255 02256 if (dmp_was_on) 02257 mpu_set_dmp_state(1); 02258 02259 return result; 02260 } 02261 02271 int mpu_write_mem(unsigned short mem_addr, unsigned short length, 02272 unsigned char *data) 02273 { 02274 unsigned char tmp[2]; 02275 02276 if (!data) 02277 return -1; 02278 if (!st.chip_cfg.sensors) 02279 return -1; 02280 02281 tmp[0] = (unsigned char)(mem_addr >> 8); 02282 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02283 02284 /* Check bank boundaries. */ 02285 if (tmp[1] + length > st.hw->bank_size) 02286 return -1; 02287 02288 if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02289 return -1; 02290 if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data)) 02291 return -1; 02292 return 0; 02293 } 02294 02304 int mpu_read_mem(unsigned short mem_addr, unsigned short length, 02305 unsigned char *data) 02306 { 02307 unsigned char tmp[2]; 02308 02309 if (!data) 02310 return -1; 02311 if (!st.chip_cfg.sensors) 02312 return -1; 02313 02314 tmp[0] = (unsigned char)(mem_addr >> 8); 02315 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02316 02317 /* Check bank boundaries. */ 02318 if (tmp[1] + length > st.hw->bank_size) 02319 return -1; 02320 02321 if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02322 return -1; 02323 if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data)) 02324 return -1; 02325 return 0; 02326 } 02327 02336 int mpu_load_firmware(unsigned short length, const unsigned char *firmware, 02337 unsigned short start_addr, unsigned short sample_rate) 02338 { 02339 unsigned short ii; 02340 unsigned short this_write; 02341 /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ 02342 #define LOAD_CHUNK (16) 02343 unsigned char cur[LOAD_CHUNK], tmp[2]; 02344 02345 if (st.chip_cfg.dmp_loaded) 02346 /* DMP should only be loaded once. */ 02347 return -1; 02348 02349 if (!firmware) 02350 return -1; 02351 for (ii = 0; ii < length; ii += this_write) { 02352 this_write = min(LOAD_CHUNK, length - ii); 02353 if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) 02354 return -1; 02355 if (mpu_read_mem(ii, this_write, cur)) 02356 return -1; 02357 if (memcmp(firmware+ii, cur, this_write)) 02358 return -2; 02359 } 02360 02361 /* Set program start address. */ 02362 tmp[0] = start_addr >> 8; 02363 tmp[1] = start_addr & 0xFF; 02364 if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) 02365 return -1; 02366 02367 st.chip_cfg.dmp_loaded = 1; 02368 st.chip_cfg.dmp_sample_rate = sample_rate; 02369 return 0; 02370 } 02371 02377 int mpu_set_dmp_state(unsigned char enable) 02378 { 02379 unsigned char tmp; 02380 if (st.chip_cfg.dmp_on == enable) 02381 return 0; 02382 02383 if (enable) { 02384 if (!st.chip_cfg.dmp_loaded) 02385 return -1; 02386 /* Disable data ready interrupt. */ 02387 set_int_enable(0); 02388 /* Disable bypass mode. */ 02389 mpu_set_bypass(0); 02390 /* Keep constant sample rate, FIFO rate controlled by DMP. */ 02391 mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); 02392 /* Remove FIFO elements. */ 02393 tmp = 0; 02394 i2c_write(st.hw->addr, 0x23, 1, &tmp); 02395 st.chip_cfg.dmp_on = 1; 02396 /* Enable DMP interrupt. */ 02397 set_int_enable(1); 02398 mpu_reset_fifo(); 02399 } else { 02400 /* Disable DMP interrupt. */ 02401 set_int_enable(0); 02402 /* Restore FIFO settings. */ 02403 tmp = st.chip_cfg.fifo_enable; 02404 i2c_write(st.hw->addr, 0x23, 1, &tmp); 02405 st.chip_cfg.dmp_on = 0; 02406 mpu_reset_fifo(); 02407 } 02408 return 0; 02409 } 02410 02416 int mpu_get_dmp_state(unsigned char *enabled) 02417 { 02418 enabled[0] = st.chip_cfg.dmp_on; 02419 return 0; 02420 } 02421 02422 02423 /* This initialization is similar to the one in ak8975.c. */ 02424 int setup_compass(void) 02425 { 02426 #ifdef AK89xx_SECONDARY 02427 unsigned char data[4], akm_addr; 02428 02429 mpu_set_bypass(1); 02430 02431 /* Find compass. Possible addresses range from 0x0C to 0x0F. */ 02432 for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { 02433 int result; 02434 result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); 02435 if (!result && (data[0] == AKM_WHOAMI)) 02436 break; 02437 } 02438 02439 if (akm_addr > 0x0F) { 02440 /* TODO: Handle this case in all compass-related functions. */ 02441 log_e("Compass not found.\n"); 02442 return -1; 02443 } 02444 02445 st.chip_cfg.compass_addr = akm_addr; 02446 02447 data[0] = AKM_POWER_DOWN; 02448 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02449 return -1; 02450 delay_ms(1); 02451 02452 data[0] = AKM_FUSE_ROM_ACCESS; 02453 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02454 return -1; 02455 delay_ms(1); 02456 02457 /* Get sensitivity adjustment data from fuse ROM. */ 02458 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) 02459 return -1; 02460 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; 02461 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; 02462 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; 02463 02464 data[0] = AKM_POWER_DOWN; 02465 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02466 return -1; 02467 delay_ms(1); 02468 02469 mpu_set_bypass(0); 02470 02471 /* Set up master mode, master clock, and ES bit. */ 02472 data[0] = 0x40; 02473 if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02474 return -1; 02475 02476 /* Slave 0 reads from AKM data registers. */ 02477 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; 02478 if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) 02479 return -1; 02480 02481 /* Compass reads start at this register. */ 02482 data[0] = AKM_REG_ST1; 02483 if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) 02484 return -1; 02485 02486 /* Enable slave 0, 8-byte reads. */ 02487 data[0] = BIT_SLAVE_EN | 8; 02488 if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) 02489 return -1; 02490 02491 /* Slave 1 changes AKM measurement mode. */ 02492 data[0] = st.chip_cfg.compass_addr; 02493 if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) 02494 return -1; 02495 02496 /* AKM measurement mode register. */ 02497 data[0] = AKM_REG_CNTL; 02498 if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) 02499 return -1; 02500 02501 /* Enable slave 1, 1-byte writes. */ 02502 data[0] = BIT_SLAVE_EN | 1; 02503 if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) 02504 return -1; 02505 02506 /* Set slave 1 data. */ 02507 data[0] = AKM_SINGLE_MEASUREMENT; 02508 if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) 02509 return -1; 02510 02511 /* Trigger slave 0 and slave 1 actions at each sample. */ 02512 data[0] = 0x03; 02513 if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) 02514 return -1; 02515 02516 #ifdef MPU9150 02517 /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ 02518 data[0] = BIT_I2C_MST_VDDIO; 02519 if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) 02520 return -1; 02521 #endif 02522 02523 return 0; 02524 #else 02525 return -1; 02526 #endif 02527 } 02528 02535 int mpu_get_compass_reg(short *data, unsigned long *timestamp) 02536 { 02537 #ifdef AK89xx_SECONDARY 02538 unsigned char tmp[9]; 02539 02540 if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 02541 return -1; 02542 02543 #ifdef AK89xx_BYPASS 02544 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) 02545 return -1; 02546 tmp[8] = AKM_SINGLE_MEASUREMENT; 02547 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) 02548 return -1; 02549 #else 02550 if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) 02551 return -1; 02552 #endif 02553 02554 #if defined AK8975_SECONDARY 02555 /* AK8975 doesn't have the overrun error bit. */ 02556 if (!(tmp[0] & AKM_DATA_READY)) 02557 return -2; 02558 if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) 02559 return -3; 02560 #elif defined AK8963_SECONDARY 02561 /* AK8963 doesn't have the data read error bit. */ 02562 if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) 02563 return -2; 02564 if (tmp[7] & AKM_OVERFLOW) 02565 return -3; 02566 #endif 02567 data[0] = (tmp[2] << 8) | tmp[1]; 02568 data[1] = (tmp[4] << 8) | tmp[3]; 02569 data[2] = (tmp[6] << 8) | tmp[5]; 02570 02571 data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; 02572 data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; 02573 data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; 02574 02575 if (timestamp) 02576 get_ms(timestamp); 02577 return 0; 02578 #else 02579 return -1; 02580 #endif 02581 } 02582 02588 int mpu_get_compass_fsr(unsigned short *fsr) 02589 { 02590 #ifdef AK89xx_SECONDARY 02591 fsr[0] = st.hw->compass_fsr; 02592 return 0; 02593 #else 02594 return -1; 02595 #endif 02596 } 02597 02642 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, 02643 unsigned char lpa_freq) 02644 { 02645 unsigned char data[3]; 02646 02647 if (lpa_freq) { 02648 unsigned char thresh_hw; 02649 02650 #if defined MPU6050 02651 /* TODO: Make these const/#defines. */ 02652 /* 1LSb = 32mg. */ 02653 if (thresh > 8160) 02654 thresh_hw = 255; 02655 else if (thresh < 32) 02656 thresh_hw = 1; 02657 else 02658 thresh_hw = thresh >> 5; 02659 #elif defined MPU6500 02660 /* 1LSb = 4mg. */ 02661 if (thresh > 1020) 02662 thresh_hw = 255; 02663 else if (thresh < 4) 02664 thresh_hw = 1; 02665 else 02666 thresh_hw = thresh >> 2; 02667 #endif 02668 02669 if (!time) 02670 /* Minimum duration must be 1ms. */ 02671 time = 1; 02672 02673 #if defined MPU6050 02674 if (lpa_freq > 40) 02675 #elif defined MPU6500 02676 if (lpa_freq > 640) 02677 #endif 02678 /* At this point, the chip has not been re-configured, so the 02679 * function can safely exit. 02680 */ 02681 return -1; 02682 02683 if (!st.chip_cfg.int_motion_only) { 02684 /* Store current settings for later. */ 02685 if (st.chip_cfg.dmp_on) { 02686 mpu_set_dmp_state(0); 02687 st.chip_cfg.cache.dmp_on = 1; 02688 } else 02689 st.chip_cfg.cache.dmp_on = 0; 02690 mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); 02691 mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); 02692 mpu_get_lpf(&st.chip_cfg.cache.lpf); 02693 mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); 02694 st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; 02695 mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); 02696 } 02697 02698 #ifdef MPU6050 02699 /* Disable hardware interrupts for now. */ 02700 set_int_enable(0); 02701 02702 /* Enter full-power accel-only mode. */ 02703 mpu_lp_accel_mode(0); 02704 02705 /* Override current LPF (and HPF) settings to obtain a valid accel 02706 * reading. 02707 */ 02708 data[0] = INV_FILTER_256HZ_NOLPF2; 02709 if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) 02710 return -1; 02711 02712 /* NOTE: Digital high pass filter should be configured here. Since this 02713 * driver doesn't modify those bits anywhere, they should already be 02714 * cleared by default. 02715 */ 02716 02717 /* Configure the device to send motion interrupts. */ 02718 /* Enable motion interrupt. */ 02719 data[0] = BIT_MOT_INT_EN; 02720 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 02721 goto lp_int_restore; 02722 02723 /* Set motion interrupt parameters. */ 02724 data[0] = thresh_hw; 02725 data[1] = time; 02726 if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data)) 02727 goto lp_int_restore; 02728 02729 /* Force hardware to "lock" current accel sample. */ 02730 delay_ms(5); 02731 data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF; 02732 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 02733 goto lp_int_restore; 02734 02735 /* Set up LP accel mode. */ 02736 data[0] = BIT_LPA_CYCLE; 02737 if (lpa_freq == 1) 02738 data[1] = INV_LPA_1_25HZ; 02739 else if (lpa_freq <= 5) 02740 data[1] = INV_LPA_5HZ; 02741 else if (lpa_freq <= 20) 02742 data[1] = INV_LPA_20HZ; 02743 else 02744 data[1] = INV_LPA_40HZ; 02745 data[1] = (data[1] << 6) | BIT_STBY_XYZG; 02746 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 02747 goto lp_int_restore; 02748 02749 st.chip_cfg.int_motion_only = 1; 02750 return 0; 02751 #elif defined MPU6500 02752 /* Disable hardware interrupts. */ 02753 set_int_enable(0); 02754 02755 /* Enter full-power accel-only mode, no FIFO/DMP. */ 02756 data[0] = 0; 02757 data[1] = 0; 02758 data[2] = BIT_STBY_XYZG; 02759 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data)) 02760 goto lp_int_restore; 02761 02762 /* Set motion threshold. */ 02763 data[0] = thresh_hw; 02764 if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data)) 02765 goto lp_int_restore; 02766 02767 /* Set wake frequency. */ 02768 if (lpa_freq == 1) 02769 data[0] = INV_LPA_1_25HZ; 02770 else if (lpa_freq == 2) 02771 data[0] = INV_LPA_2_5HZ; 02772 else if (lpa_freq <= 5) 02773 data[0] = INV_LPA_5HZ; 02774 else if (lpa_freq <= 10) 02775 data[0] = INV_LPA_10HZ; 02776 else if (lpa_freq <= 20) 02777 data[0] = INV_LPA_20HZ; 02778 else if (lpa_freq <= 40) 02779 data[0] = INV_LPA_40HZ; 02780 else if (lpa_freq <= 80) 02781 data[0] = INV_LPA_80HZ; 02782 else if (lpa_freq <= 160) 02783 data[0] = INV_LPA_160HZ; 02784 else if (lpa_freq <= 320) 02785 data[0] = INV_LPA_320HZ; 02786 else 02787 data[0] = INV_LPA_640HZ; 02788 if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) 02789 goto lp_int_restore; 02790 02791 /* Enable motion interrupt (MPU6500 version). */ 02792 data[0] = BITS_WOM_EN; 02793 if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) 02794 goto lp_int_restore; 02795 02796 /* Enable cycle mode. */ 02797 data[0] = BIT_LPA_CYCLE; 02798 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 02799 goto lp_int_restore; 02800 02801 /* Enable interrupt. */ 02802 data[0] = BIT_MOT_INT_EN; 02803 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 02804 goto lp_int_restore; 02805 02806 st.chip_cfg.int_motion_only = 1; 02807 return 0; 02808 #endif 02809 } else { 02810 /* Don't "restore" the previous state if no state has been saved. */ 02811 int ii; 02812 char *cache_ptr = (char*)&st.chip_cfg.cache; 02813 for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { 02814 if (cache_ptr[ii] != 0) 02815 goto lp_int_restore; 02816 } 02817 /* If we reach this point, motion interrupt mode hasn't been used yet. */ 02818 return -1; 02819 } 02820 lp_int_restore: 02821 /* Set to invalid values to ensure no I2C writes are skipped. */ 02822 st.chip_cfg.gyro_fsr = 0xFF; 02823 st.chip_cfg.accel_fsr = 0xFF; 02824 st.chip_cfg.lpf = 0xFF; 02825 st.chip_cfg.sample_rate = 0xFFFF; 02826 st.chip_cfg.sensors = 0xFF; 02827 st.chip_cfg.fifo_enable = 0xFF; 02828 st.chip_cfg.clk_src = INV_CLK_PLL; 02829 mpu_set_sensors(st.chip_cfg.cache.sensors_on); 02830 mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); 02831 mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); 02832 mpu_set_lpf(st.chip_cfg.cache.lpf); 02833 mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); 02834 mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); 02835 02836 if (st.chip_cfg.cache.dmp_on) 02837 mpu_set_dmp_state(1); 02838 02839 #ifdef MPU6500 02840 /* Disable motion interrupt (MPU6500 version). */ 02841 data[0] = 0; 02842 if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) 02843 goto lp_int_restore; 02844 #endif 02845 02846 st.chip_cfg.int_motion_only = 0; 02847 return 0; 02848 } 02850 //添加的代码部分 02852 //本程序只供学习使用，未经作者许可，不得用于其它任何用途 02853 //ALIENTEK精英STM32开发板V3 02854 //MPU6050 DMP 驱动代码 02855 //正点原子@ALIENTEK 02856 //技术论坛:www.openedv.com 02857 //创建日期:2015/1/17 02858 //版本：V1.0 02859 //版权所有，盗版必究。 02860 //Copyright(C) 广州市星翼电子科技有限公司 2009-2019 02861 //All rights reserved 02863 02864 //q30格式,long转float时的除数. 02865 #define q30 1073741824.0f 02866 02867 //陀螺仪方向设置 02868 static signed char gyro_orientation[9] = { 1, 0, 0, 02869 0, 1, 0, 02870 0, 0, 1}; 02871 //MPU6050自测试 02872 //返回值:0,正常 02873 // 其他,失败 02874 u8 run_self_test(void){ 02875 int result; 02876 //char test_packet[4] = {0}; 02877 long gyro[3], accel[3]; 02878 result = mpu_run_self_test(gyro, accel); 02879 if (result == 0x3){ 02880 /* Test passed. We can trust the gyro data here, so let's push it down 02881 * to the DMP. 02882 */ 02883 float sens; 02884 unsigned short accel_sens; 02885 mpu_get_gyro_sens(&sens); 02886 gyro[0] = (long)(gyro[0] * sens); 02887 gyro[1] = (long)(gyro[1] * sens); 02888 gyro[2] = (long)(gyro[2] * sens); 02889 dmp_set_gyro_bias(gyro); 02890 mpu_get_accel_sens(&accel_sens); 02891 accel[0] *= accel_sens; 02892 accel[1] *= accel_sens; 02893 accel[2] *= accel_sens; 02894 dmp_set_accel_bias(accel); 02895 return 0; 02896 }else return 1;} 02897 //陀螺仪方向控制 02898 unsigned short inv_orientation_matrix_to_scalar( 02899 const signed char *mtx){ 02900 unsigned short scalar; 02901 /* 02902 XYZ 010_001_000 Identity Matrix 02903 XZY 001_010_000 02904 YXZ 010_000_001 02905 YZX 000_010_001 02906 ZXY 001_000_010 02907 ZYX 000_001_010 02908 */ 02909 scalar = inv_row_2_scale(mtx); 02910 scalar |= inv_row_2_scale(mtx + 3) << 3; 02911 scalar |= inv_row_2_scale(mtx + 6) << 6; 02912 return scalar;} 02913 //方向转换 02914 unsigned short inv_row_2_scale(const signed char *row){ 02915 unsigned short b; 02916 02917 if (row[0] > 0) 02918 b = 0; 02919 else if (row[0] < 0) 02920 b = 4; 02921 else if (row[1] > 0) 02922 b = 1; 02923 else if (row[1] < 0) 02924 b = 5; 02925 else if (row[2] > 0) 02926 b = 2; 02927 else if (row[2] < 0) 02928 b = 6; 02929 else 02930 b = 7; // error 02931 return b;} 02932 //空函数,未用到. 02933 void mget_ms(unsigned long *time){} 02934 //mpu6050,dmp初始化 02935 //返回值:0,正常 02936 // 其他,失败 02937 u8 mpu_dmp_init(void) 02938 { 02939 u8 res=0; 02940 if(mpu_init()==0) //初始化MPU6050 02941 { 02942 res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); //设置所需要的传感器 02943 if(res)return 1; 02944 res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); //设置FIFO 02945 if(res)return 2; 02946 res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 02947 if(res)return 3; 02948 res=dmp_load_motion_driver_firmware(); //加载dmp固件 02949 if(res)return 4; 02950 res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向 02951 if(res)return 5; 02952 res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能 02953 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| 02954 DMP_FEATURE_GYRO_CAL); 02955 if(res)return 6; 02956 res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz) 02957 if(res)return 7; 02958 res=run_self_test(); //自检 02959 if(res)return 8; 02960 res=mpu_set_dmp_state(1); //使能DMP 02961 if(res)return 9; 02962 }else return 10; 02963 return 0;} 02964 //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多) 02965 //pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0° 02966 //roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0° 02967 //yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0° 02968 //返回值:0,正常 02969 // 其他,失败 02970 u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw){ 02971 float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; 02972 unsigned long sensor_timestamp; 02973 short gyro[3], accel[3], sensors; 02974 unsigned char more; 02975 long quat[4]; 02976 if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; 02977 /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units. 02978 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent. 02979 **/ 02980 /*if (sensors & INV_XYZ_GYRO ) 02981 send_packet(PACKET_TYPE_GYRO, gyro); 02982 if (sensors & INV_XYZ_ACCEL) 02983 send_packet(PACKET_TYPE_ACCEL, accel); */ 02984 /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. 02985 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 02986 **/ 02987 if(sensors&INV_WXYZ_QUAT) 02988 { 02989 q0 = quat[0] / q30; //q30格式转换为浮点数 02990 q1 = quat[1] / q30; 02991 q2 = quat[2] / q30; 02992 q3 = quat[3] / q30; 02993 //计算得到俯仰角/横滚角/航向角 02994 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 02995 *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll 02996 *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw 02997 }else return 2; 02998 return 0; 02999 }