Chen Huan
平衡车的MPU6050驱动 C.H.
Embed:
(wiki syntax)
Show/hide line numbers
mpu6050.cpp
00001 #include <stdio.h> 00002 #include <stdint.h> 00003 #include <stdlib.h> 00004 #include <string.h> 00005 #include <math.h> 00006 #include "mpu6050.h" 00007 00008 ////////////////////////////////////////////////////////////////////////////////// 00009 //MPU6050驱动程序 C.H. 00010 ////////////////////////////////////////////////////////////////////////////////// 00011 00012 00013 00014 //初始化MPU6050 00015 //返回值:0,成功 00016 // 其他,错误代码 00017 unsigned char MPU_Init(void) 00018 { 00019 unsigned char res; 00020 MPU_IIC_Init();//初始化IIC总线 00021 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050 00022 delay_ms(100); 00023 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050 00024 MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps 00025 MPU_Set_Accel_Fsr(0); //加速度传感器,±2g 00026 MPU_Set_Rate(50); //设置采样率50Hz 00027 MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断 00028 MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭 00029 MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO 00030 MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效 00031 res=MPU_Read_Byte(MPU_DEVICE_ID_REG); 00032 if(res==MPU_ADDR)//器件ID正确 00033 { 00034 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考 00035 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作 00036 MPU_Set_Rate(50); //设置采样率为50Hz 00037 }else return 1; 00038 return 0; 00039 } 00040 00041 00042 00043 //设置MPU6050陀螺仪传感器满量程范围 00044 //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps 00045 //返回值:0,设置成功 00046 // 其他,设置失败 00047 unsigned char MPU_Set_Gyro_Fsr(unsigned char fsr) 00048 { 00049 return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围 00050 } 00051 //设置MPU6050加速度传感器满量程范围 00052 //fsr:0,±2g;1,±4g;2,±8g;3,±16g 00053 //返回值:0,设置成功 00054 // 其他,设置失败 00055 unsigned char MPU_Set_Accel_Fsr(unsigned char fsr) 00056 { 00057 return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围 00058 } 00059 //设置MPU6050的数字低通滤波器 00060 //lpf:数字低通滤波频率(Hz) 00061 //返回值:0,设置成功 00062 // 其他,设置失败 00063 unsigned char MPU_Set_LPF(unsigned short lpf) 00064 { 00065 unsigned char data=0; 00066 if(lpf>=188)data=1; 00067 else if(lpf>=98)data=2; 00068 else if(lpf>=42)data=3; 00069 else if(lpf>=20)data=4; 00070 else if(lpf>=10)data=5; 00071 else data=6; 00072 return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器 00073 } 00074 //设置MPU6050的采样率(假定Fs=1KHz) 00075 //rate:4~1000(Hz) 00076 //返回值:0,设置成功 00077 // 其他,设置失败 00078 unsigned char MPU_Set_Rate(unsigned short rate) 00079 { 00080 unsigned char data; 00081 if(rate>1000)rate=1000; 00082 if(rate<4)rate=4; 00083 data=1000/rate-1; 00084 data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器 00085 return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半 00086 } 00087 00088 //得到温度值 00089 //返回值:温度值(扩大了100倍) 00090 short MPU_Get_Temperature(void) 00091 { 00092 unsigned char buf[2]; 00093 short raw; 00094 float temp; 00095 MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 00096 raw=((unsigned short )buf[0]<<8)|buf[1]; 00097 temp=36.53+((double)raw)/340; 00098 return temp*100;; 00099 } 00100 //得到陀螺仪值(原始值) 00101 //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) 00102 //返回值:0,成功 00103 // 其他,错误代码 00104 unsigned char MPU_Get_Gyroscope(short *gx,short *gy,short *gz) 00105 { 00106 unsigned char buf[6],res; 00107 res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf); 00108 if(res==0) 00109 { 00110 *gx=((unsigned short )buf[0]<<8)|buf[1]; 00111 *gy=((unsigned short )buf[2]<<8)|buf[3]; 00112 *gz=((unsigned short )buf[4]<<8)|buf[5]; 00113 } 00114 return res;; 00115 } 00116 //得到加速度值(原始值) 00117 //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) 00118 //返回值:0,成功 00119 // 其他,错误代码 00120 unsigned char MPU_Get_Accelerometer(short *ax,short *ay,short *az) 00121 { 00122 unsigned char buf[6],res; 00123 res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf); 00124 if(res==0) 00125 { 00126 *ax=((unsigned short )buf[0]<<8)|buf[1]; 00127 *ay=((unsigned short )buf[2]<<8)|buf[3]; 00128 *az=((unsigned short )buf[4]<<8)|buf[5]; 00129 } 00130 return res;; 00131 } 00132 //IIC连续写 00133 //addr:器件地址 00134 //reg:寄存器地址 00135 //len:写入长度 00136 //buf:数据区 00137 //返回值:0,正常 00138 // 其他,错误代码 00139 unsigned char MPU_Write_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf) 00140 { 00141 unsigned char i; 00142 MPU_IIC_Start(); 00143 MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 00144 if(MPU_IIC_Wait_Ack()) //等待应答 00145 { 00146 MPU_IIC_Stop(); 00147 return 1; 00148 } 00149 MPU_IIC_Send_Byte(reg); //写寄存器地址 00150 MPU_IIC_Wait_Ack(); //等待应答 00151 for(i=0;i<len;i++) 00152 { 00153 MPU_IIC_Send_Byte(buf[i]); //发送数据 00154 if(MPU_IIC_Wait_Ack()) //等待ACK 00155 { 00156 MPU_IIC_Stop(); 00157 return 1; 00158 } 00159 } 00160 MPU_IIC_Stop(); 00161 return 0; 00162 } 00163 //IIC连续读 00164 //addr:器件地址 00165 //reg:要读取的寄存器地址 00166 //len:要读取的长度 00167 //buf:读取到的数据存储区 00168 //返回值:0,正常 00169 // 其他,错误代码 00170 unsigned char MPU_Read_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf) 00171 { 00172 MPU_IIC_Start(); 00173 MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 00174 if(MPU_IIC_Wait_Ack()) //等待应答 00175 { 00176 MPU_IIC_Stop(); 00177 return 1; 00178 } 00179 MPU_IIC_Send_Byte(reg); //写寄存器地址 00180 MPU_IIC_Wait_Ack(); //等待应答 00181 MPU_IIC_Start(); 00182 MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令 00183 MPU_IIC_Wait_Ack(); //等待应答 00184 while(len) 00185 { 00186 if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK 00187 else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK 00188 len--; 00189 buf++; 00190 } 00191 MPU_IIC_Stop(); //产生一个停止条件 00192 return 0; 00193 } 00194 //IIC写一个字节 00195 //reg:寄存器地址 00196 //data:数据 00197 //返回值:0,正常 00198 // 其他,错误代码 00199 unsigned char MPU_Write_Byte(unsigned char reg,unsigned char data) 00200 { 00201 MPU_IIC_Start(); 00202 MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 00203 if(MPU_IIC_Wait_Ack()) //等待应答 00204 { 00205 MPU_IIC_Stop(); 00206 return 1; 00207 } 00208 MPU_IIC_Send_Byte(reg); //写寄存器地址 00209 MPU_IIC_Wait_Ack(); //等待应答 00210 MPU_IIC_Send_Byte(data);//发送数据 00211 if(MPU_IIC_Wait_Ack()) //等待ACK 00212 { 00213 MPU_IIC_Stop(); 00214 return 1; 00215 } 00216 MPU_IIC_Stop(); 00217 return 0; 00218 } 00219 //IIC读一个字节 00220 //reg:寄存器地址 00221 //返回值:读到的数据 00222 unsigned char MPU_Read_Byte(unsigned char reg) 00223 { 00224 unsigned char res; 00225 MPU_IIC_Start(); 00226 MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 00227 MPU_IIC_Wait_Ack(); //等待应答 00228 MPU_IIC_Send_Byte(reg); //写寄存器地址 00229 MPU_IIC_Wait_Ack(); //等待应答 00230 MPU_IIC_Start(); 00231 MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令 00232 MPU_IIC_Wait_Ack(); //等待应答 00233 res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK 00234 MPU_IIC_Stop(); //产生一个停止条件 00235 return res; 00236 }/**/ 00237 00238 00239 //MPU IIC 延时函数 00240 void MPU_IIC_Delay(void) 00241 { 00242 delay_us(2); 00243 } 00244 //初始化IIC 00245 void MPU_IIC_Init(void) 00246 { 00247 00248 00249 #if defined DRIVER_MODE_BALANCE 00250 RCC->APB2ENR|=1<<3; //先使能外设IO PORTC时钟 00251 GPIOB->CRL&=0X00FFFFFF; //PC11/12 推挽输出 00252 GPIOB->CRL|=0X33000000; 00253 GPIOB->ODR|=3<<6; //PC11,12 输出高 00254 #elif defined DRIVER_MODE_ROTOR 00255 RCC->APB2ENR|=1<<3; //先使能外设IO PORTC时钟 00256 GPIOB->CRH&=0XFFF00FFF; //PC11/12 推挽输出 00257 GPIOB->CRH|=0X00033000; 00258 GPIOB->ODR|=3<<11; //PC11,12 输出高 00259 #else 00260 #error Target Board is not specified. 00261 #endif 00262 } 00263 //产生IIC起始信号 00264 void MPU_IIC_Start(void) 00265 { 00266 MPU_SDA_OUT(); //sda线输出 00267 MPU_IIC_SDA=1; 00268 MPU_IIC_SCL=1; 00269 MPU_IIC_Delay(); 00270 MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low 00271 MPU_IIC_Delay(); 00272 MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据 00273 } 00274 //产生IIC停止信号 00275 void MPU_IIC_Stop(void) 00276 { 00277 MPU_SDA_OUT();//sda线输出 00278 MPU_IIC_SCL=0; 00279 MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high 00280 MPU_IIC_Delay(); 00281 MPU_IIC_SCL=1; 00282 MPU_IIC_SDA=1;//发送I2C总线结束信号 00283 MPU_IIC_Delay(); 00284 } 00285 //等待应答信号到来 00286 //返回值：1，接收应答失败 00287 // 0，接收应答成功 00288 unsigned char MPU_IIC_Wait_Ack(void) 00289 { 00290 unsigned char ucErrTime=0; 00291 MPU_SDA_IN(); //SDA设置为输入 00292 MPU_IIC_SDA=1;MPU_IIC_Delay(); 00293 MPU_IIC_SCL=1;MPU_IIC_Delay(); 00294 while(MPU_READ_SDA) 00295 { 00296 ucErrTime++; 00297 if(ucErrTime>250) 00298 { 00299 MPU_IIC_Stop(); 00300 return 1; 00301 } 00302 } 00303 MPU_IIC_SCL=0;//时钟输出0 00304 return 0; 00305 } 00306 //产生ACK应答 00307 void MPU_IIC_Ack(void) 00308 { 00309 MPU_IIC_SCL=0; 00310 MPU_SDA_OUT(); 00311 MPU_IIC_SDA=0; 00312 MPU_IIC_Delay(); 00313 MPU_IIC_SCL=1; 00314 MPU_IIC_Delay(); 00315 MPU_IIC_SCL=0; 00316 } 00317 //不产生ACK应答 00318 void MPU_IIC_NAck(void) 00319 { 00320 MPU_IIC_SCL=0; 00321 MPU_SDA_OUT(); 00322 MPU_IIC_SDA=1; 00323 MPU_IIC_Delay(); 00324 MPU_IIC_SCL=1; 00325 MPU_IIC_Delay(); 00326 MPU_IIC_SCL=0; 00327 } 00328 //IIC发送一个字节 00329 //返回从机有无应答 00330 //1，有应答 00331 //0，无应答 00332 void MPU_IIC_Send_Byte(unsigned char txd) 00333 { 00334 unsigned char t; 00335 MPU_SDA_OUT(); 00336 MPU_IIC_SCL=0;//拉低时钟开始数据传输 00337 for(t=0;t<8;t++) 00338 { 00339 MPU_IIC_SDA=(txd&0x80)>>7; 00340 txd<<=1; 00341 MPU_IIC_SCL=1; 00342 MPU_IIC_Delay(); 00343 MPU_IIC_SCL=0; 00344 MPU_IIC_Delay(); 00345 } 00346 } 00347 //读1个字节，ack=1时，发送ACK，ack=0，发送nACK 00348 unsigned char MPU_IIC_Read_Byte(unsigned char ack) 00349 { 00350 unsigned char i,receive=0; 00351 MPU_SDA_IN();//SDA设置为输入 00352 for(i=0;i<8;i++ ) 00353 { 00354 MPU_IIC_SCL=0; 00355 MPU_IIC_Delay(); 00356 MPU_IIC_SCL=1; 00357 receive<<=1; 00358 if(MPU_READ_SDA)receive++; 00359 MPU_IIC_Delay(); 00360 } 00361 if (!ack) 00362 MPU_IIC_NAck();//发送nACK 00363 else 00364 MPU_IIC_Ack(); //发送ACK 00365 return receive; 00366 } 00367 00368 #define MPU6050 //定义我们使用的传感器为MPU6050 00369 #define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1) 00370 00371 /* The following functions must be defined for this platform: 00372 * i2c_write(unsigned char slave_addr, unsigned char reg_addr, 00373 * unsigned char length, unsigned char const *data) 00374 * i2c_read(unsigned char slave_addr, unsigned char reg_addr, 00375 * unsigned char length, unsigned char *data) 00376 * delay_ms(unsigned long num_ms) 00377 * get_ms(unsigned long *count) 00378 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) 00379 * labs(long x) 00380 * fabsf(float x) 00381 * min(int a, int b) 00382 */ 00383 #if defined MOTION_DRIVER_TARGET_MSP430 00384 //#include "msp430.h" 00385 //#include "msp430_i2c.h" 00386 //#include "msp430_clock.h" 00387 //#include "msp430_interrupt.h" 00388 00389 #define i2c_write MPU_Write_Len 00390 #define i2c_read MPU_Read_Len 00391 //#define delay_ms delay_ms 00392 #define get_ms mget_ms 00393 //static inline int reg_int_cb(struct int_param_s *int_param) 00394 //{ 00395 // return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00396 // int_param->active_low); 00397 //} 00398 ////#define // printf //打印信息 00399 ////#define // printf //打印信息 00400 /* labs is already defined by TI's toolchain. */ 00401 /* fabs is for doubles. fabsf is for floats. */ 00402 #define fabs fabsf 00403 #define min(a,b) ((a<b)?a:b) 00404 #elif defined EMPL_TARGET_MSP430 00405 #include "msp430.h" 00406 #include "msp430_i2c.h" 00407 #include "msp430_clock.h" 00408 #include "msp430_interrupt.h" 00409 #include "log.h" 00410 #define i2c_write msp430_i2c_write 00411 #define i2c_read msp430_i2c_read 00412 #define delay_ms msp430_delay_ms 00413 #define get_ms msp430_get_clock_ms 00414 static inline int reg_int_cb(struct int_param_s *int_param) 00415 { 00416 return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, 00417 int_param->active_low); 00418 } 00419 //#define // MPL_LOGI 00420 //#define // MPL_LOGE 00421 /* labs is already defined by TI's toolchain. */ 00422 /* fabs is for doubles. fabsf is for floats. */ 00423 #define fabs fabsf 00424 #define min(a,b) ((a<b)?a:b) 00425 #elif defined EMPL_TARGET_UC3L0 00426 /* Instead of using the standard TWI driver from the ASF library, we're using 00427 * a TWI driver that follows the slave address + register address convention. 00428 */ 00429 #include "twi.h" 00430 #include "delay.h" 00431 #include "sysclk.h" 00432 #include "log.h" 00433 #include "sensors_xplained.h" 00434 #include "uc3l0_clock.h" 00435 #define i2c_write(a, b, c, d) twi_write(a, b, d, c) 00436 #define i2c_read(a, b, c, d) twi_read(a, b, d, c) 00437 /* delay_ms is a function already defined in ASF. */ 00438 #define get_ms uc3l0_get_clock_ms 00439 static inline int reg_int_cb(struct int_param_s *int_param) 00440 { 00441 sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg); 00442 return 0; 00443 } 00444 //#define // MPL_LOGI 00445 //#define // MPL_LOGE 00446 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */ 00447 #define labs abs 00448 #define fabs(x) (((x)>0)?(x):-(x)) 00449 #else 00450 #error Gyro driver is missing the system layer implementations. 00451 #endif 00452 00453 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 00454 #error Which gyro are you using? Define MPUxxxx in your compiler options. 00455 #endif 00456 00457 /* Time for some messy macro work. =] 00458 * #define MPU9150 00459 * is equivalent to.. 00460 * #define MPU6050 00461 * #define AK8975_SECONDARY 00462 * 00463 * #define MPU9250 00464 * is equivalent to.. 00465 * #define MPU6500 00466 * #define AK8963_SECONDARY 00467 */ 00468 #if defined MPU9150 00469 #ifndef MPU6050 00470 #define MPU6050 00471 #endif /* #ifndef MPU6050 */ 00472 #if defined AK8963_SECONDARY 00473 #error "MPU9150 and AK8963_SECONDARY cannot both be defined." 00474 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ 00475 #define AK8975_SECONDARY 00476 #endif /* #if defined AK8963_SECONDARY */ 00477 #elif defined MPU9250 /* #if defined MPU9150 */ 00478 #ifndef MPU6500 00479 #define MPU6500 00480 #endif /* #ifndef MPU6500 */ 00481 #if defined AK8975_SECONDARY 00482 #error "MPU9250 and AK8975_SECONDARY cannot both be defined." 00483 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ 00484 #define AK8963_SECONDARY 00485 #endif /* #if defined AK8975_SECONDARY */ 00486 #endif /* #if defined MPU9150 */ 00487 00488 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY 00489 #define AK89xx_SECONDARY 00490 #else 00491 /* #warning "No compass = less profit for Invensense. Lame." */ 00492 #endif 00493 00494 static int set_int_enable(unsigned char enable); 00495 00496 /* Hardware registers needed by driver. */ 00497 struct gyro_reg_s { 00498 unsigned char who_am_i; 00499 unsigned char rate_div; 00500 unsigned char lpf; 00501 unsigned char prod_id; 00502 unsigned char user_ctrl; 00503 unsigned char fifo_en; 00504 unsigned char gyro_cfg; 00505 unsigned char accel_cfg; 00506 // unsigned char accel_cfg2; 00507 // unsigned char lp_accel_odr; 00508 unsigned char motion_thr; 00509 unsigned char motion_dur; 00510 unsigned char fifo_count_h; 00511 unsigned char fifo_r_w; 00512 unsigned char raw_gyro; 00513 unsigned char raw_accel; 00514 unsigned char temp; 00515 unsigned char int_enable; 00516 unsigned char dmp_int_status; 00517 unsigned char int_status; 00518 // unsigned char accel_intel; 00519 unsigned char pwr_mgmt_1; 00520 unsigned char pwr_mgmt_2; 00521 unsigned char int_pin_cfg; 00522 unsigned char mem_r_w; 00523 unsigned char accel_offs; 00524 unsigned char i2c_mst; 00525 unsigned char bank_sel; 00526 unsigned char mem_start_addr; 00527 unsigned char prgm_start_h; 00528 #if defined AK89xx_SECONDARY 00529 unsigned char s0_addr; 00530 unsigned char s0_reg; 00531 unsigned char s0_ctrl; 00532 unsigned char s1_addr; 00533 unsigned char s1_reg; 00534 unsigned char s1_ctrl; 00535 unsigned char s4_ctrl; 00536 unsigned char s0_do; 00537 unsigned char s1_do; 00538 unsigned char i2c_delay_ctrl; 00539 unsigned char raw_compass; 00540 /* The I2C_MST_VDDIO bit is in this register. */ 00541 unsigned char yg_offs_tc; 00542 #endif 00543 }; 00544 00545 /* Information specific to a particular device. */ 00546 struct hw_s { 00547 unsigned char addr; 00548 unsigned short max_fifo; 00549 unsigned char num_reg; 00550 unsigned short temp_sens; 00551 short temp_offset; 00552 unsigned short bank_size; 00553 #if defined AK89xx_SECONDARY 00554 unsigned short compass_fsr; 00555 #endif 00556 }; 00557 00558 /* When entering motion interrupt mode, the driver keeps track of the 00559 * previous state so that it can be restored at a later time. 00560 * TODO: This is tacky. Fix it. 00561 */ 00562 struct motion_int_cache_s { 00563 unsigned short gyro_fsr; 00564 unsigned char accel_fsr; 00565 unsigned short lpf; 00566 unsigned short sample_rate; 00567 unsigned char sensors_on; 00568 unsigned char fifo_sensors; 00569 unsigned char dmp_on; 00570 }; 00571 00572 /* Cached chip configuration data. 00573 * TODO: A lot of these can be handled with a bitmask. 00574 */ 00575 struct chip_cfg_s { 00576 /* Matches gyro_cfg >> 3 & 0x03 */ 00577 unsigned char gyro_fsr; 00578 /* Matches accel_cfg >> 3 & 0x03 */ 00579 unsigned char accel_fsr; 00580 /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ 00581 unsigned char sensors; 00582 /* Matches config register. */ 00583 unsigned char lpf; 00584 unsigned char clk_src; 00585 /* Sample rate, NOT rate divider. */ 00586 unsigned short sample_rate; 00587 /* Matches fifo_en register. */ 00588 unsigned char fifo_enable; 00589 /* Matches int enable register. */ 00590 unsigned char int_enable; 00591 /* 1 if devices on auxiliary I2C bus appear on the primary. */ 00592 unsigned char bypass_mode; 00593 /* 1 if half-sensitivity. 00594 * NOTE: This doesn't belong here, but everything else in hw_s is const, 00595 * and this allows us to save some precious RAM. 00596 */ 00597 unsigned char accel_half; 00598 /* 1 if device in low-power accel-only mode. */ 00599 unsigned char lp_accel_mode; 00600 /* 1 if interrupts are only triggered on motion events. */ 00601 unsigned char int_motion_only; 00602 struct motion_int_cache_s cache; 00603 /* 1 for active low interrupts. */ 00604 unsigned char active_low_int; 00605 /* 1 for latched interrupts. */ 00606 unsigned char latched_int; 00607 /* 1 if DMP is enabled. */ 00608 unsigned char dmp_on; 00609 /* Ensures that DMP will only be loaded once. */ 00610 unsigned char dmp_loaded; 00611 /* Sampling rate used when DMP is enabled. */ 00612 unsigned short dmp_sample_rate; 00613 #ifdef AK89xx_SECONDARY 00614 /* Compass sample rate. */ 00615 unsigned short compass_sample_rate; 00616 unsigned char compass_addr; 00617 short mag_sens_adj[3]; 00618 #endif 00619 }; 00620 00621 /* Information for self-test. */ 00622 struct test_s { 00623 unsigned long gyro_sens; 00624 unsigned long accel_sens; 00625 unsigned char reg_rate_div; 00626 unsigned char reg_lpf; 00627 unsigned char reg_gyro_fsr; 00628 unsigned char reg_accel_fsr; 00629 unsigned short wait_ms; 00630 unsigned char packet_thresh; 00631 float min_dps; 00632 float max_dps; 00633 float max_gyro_var; 00634 float min_g; 00635 float max_g; 00636 float max_accel_var; 00637 }; 00638 00639 /* Gyro driver state variables. */ 00640 struct gyro_state_s { 00641 const struct gyro_reg_s *reg; 00642 const struct hw_s *hw; 00643 struct chip_cfg_s chip_cfg; 00644 const struct test_s *test; 00645 }; 00646 00647 /* Filter configurations. */ 00648 enum lpf_e { 00649 INV_FILTER_256HZ_NOLPF2 = 0, 00650 INV_FILTER_188HZ, 00651 INV_FILTER_98HZ, 00652 INV_FILTER_42HZ, 00653 INV_FILTER_20HZ, 00654 INV_FILTER_10HZ, 00655 INV_FILTER_5HZ, 00656 INV_FILTER_2100HZ_NOLPF, 00657 NUM_FILTER 00658 }; 00659 00660 /* Full scale ranges. */ 00661 enum gyro_fsr_e { 00662 INV_FSR_250DPS = 0, 00663 INV_FSR_500DPS, 00664 INV_FSR_1000DPS, 00665 INV_FSR_2000DPS, 00666 NUM_GYRO_FSR 00667 }; 00668 00669 /* Full scale ranges. */ 00670 enum accel_fsr_e { 00671 INV_FSR_2G = 0, 00672 INV_FSR_4G, 00673 INV_FSR_8G, 00674 INV_FSR_16G, 00675 NUM_ACCEL_FSR 00676 }; 00677 00678 /* Clock sources. */ 00679 enum clock_sel_e { 00680 INV_CLK_INTERNAL = 0, 00681 INV_CLK_PLL, 00682 NUM_CLK 00683 }; 00684 00685 /* Low-power accel wakeup rates. */ 00686 enum lp_accel_rate_e { 00687 #if defined MPU6050 00688 INV_LPA_1_25HZ, 00689 INV_LPA_5HZ, 00690 INV_LPA_20HZ, 00691 INV_LPA_40HZ 00692 #elif defined MPU6500 00693 INV_LPA_0_3125HZ, 00694 INV_LPA_0_625HZ, 00695 INV_LPA_1_25HZ, 00696 INV_LPA_2_5HZ, 00697 INV_LPA_5HZ, 00698 INV_LPA_10HZ, 00699 INV_LPA_20HZ, 00700 INV_LPA_40HZ, 00701 INV_LPA_80HZ, 00702 INV_LPA_160HZ, 00703 INV_LPA_320HZ, 00704 INV_LPA_640HZ 00705 #endif 00706 }; 00707 00708 #define BIT_I2C_MST_VDDIO (0x80) 00709 #define BIT_FIFO_EN (0x40) 00710 #define BIT_DMP_EN (0x80) 00711 #define BIT_FIFO_RST (0x04) 00712 #define BIT_DMP_RST (0x08) 00713 #define BIT_FIFO_OVERFLOW (0x10) 00714 #define BIT_DATA_RDY_EN (0x01) 00715 #define BIT_DMP_INT_EN (0x02) 00716 #define BIT_MOT_INT_EN (0x40) 00717 #define BITS_FSR (0x18) 00718 #define BITS_LPF (0x07) 00719 #define BITS_HPF (0x07) 00720 #define BITS_CLK (0x07) 00721 #define BIT_FIFO_SIZE_1024 (0x40) 00722 #define BIT_FIFO_SIZE_2048 (0x80) 00723 #define BIT_FIFO_SIZE_4096 (0xC0) 00724 #define BIT_RESET (0x80) 00725 #define BIT_SLEEP (0x40) 00726 #define BIT_S0_DELAY_EN (0x01) 00727 #define BIT_S2_DELAY_EN (0x04) 00728 #define BITS_SLAVE_LENGTH (0x0F) 00729 #define BIT_SLAVE_BYTE_SW (0x40) 00730 #define BIT_SLAVE_GROUP (0x10) 00731 #define BIT_SLAVE_EN (0x80) 00732 #define BIT_I2C_READ (0x80) 00733 #define BITS_I2C_MASTER_DLY (0x1F) 00734 #define BIT_AUX_IF_EN (0x20) 00735 #define BIT_ACTL (0x80) 00736 #define BIT_LATCH_EN (0x20) 00737 #define BIT_ANY_RD_CLR (0x10) 00738 #define BIT_BYPASS_EN (0x02) 00739 #define BITS_WOM_EN (0xC0) 00740 #define BIT_LPA_CYCLE (0x20) 00741 #define BIT_STBY_XA (0x20) 00742 #define BIT_STBY_YA (0x10) 00743 #define BIT_STBY_ZA (0x08) 00744 #define BIT_STBY_XG (0x04) 00745 #define BIT_STBY_YG (0x02) 00746 #define BIT_STBY_ZG (0x01) 00747 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) 00748 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) 00749 00750 #if defined AK8975_SECONDARY 00751 #define SUPPORTS_AK89xx_HIGH_SENS (0x00) 00752 #define AK89xx_FSR (9830) 00753 #elif defined AK8963_SECONDARY 00754 #define SUPPORTS_AK89xx_HIGH_SENS (0x10) 00755 #define AK89xx_FSR (4915) 00756 #endif 00757 00758 #ifdef AK89xx_SECONDARY 00759 #define AKM_REG_WHOAMI (0x00) 00760 00761 #define AKM_REG_ST1 (0x02) 00762 #define AKM_REG_HXL (0x03) 00763 #define AKM_REG_ST2 (0x09) 00764 00765 #define AKM_REG_CNTL (0x0A) 00766 #define AKM_REG_ASTC (0x0C) 00767 #define AKM_REG_ASAX (0x10) 00768 #define AKM_REG_ASAY (0x11) 00769 #define AKM_REG_ASAZ (0x12) 00770 00771 #define AKM_DATA_READY (0x01) 00772 #define AKM_DATA_OVERRUN (0x02) 00773 #define AKM_OVERFLOW (0x80) 00774 #define AKM_DATA_ERROR (0x40) 00775 00776 #define AKM_BIT_SELF_TEST (0x40) 00777 00778 #define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) 00779 #define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) 00780 #define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) 00781 #define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) 00782 00783 #define AKM_WHOAMI (0x48) 00784 #endif 00785 00786 #if defined MPU6050 00787 //const struct gyro_reg_s reg = { 00788 // .who_am_i = 0x75, 00789 // .rate_div = 0x19, 00790 // .lpf = 0x1A, 00791 // .prod_id = 0x0C, 00792 // .user_ctrl = 0x6A, 00793 // .fifo_en = 0x23, 00794 // .gyro_cfg = 0x1B, 00795 // .accel_cfg = 0x1C, 00796 // .motion_thr = 0x1F, 00797 // .motion_dur = 0x20, 00798 // .fifo_count_h = 0x72, 00799 // .fifo_r_w = 0x74, 00800 // .raw_gyro = 0x43, 00801 // .raw_accel = 0x3B, 00802 // .temp = 0x41, 00803 // .int_enable = 0x38, 00804 // .dmp_int_status = 0x39, 00805 // .int_status = 0x3A, 00806 // .pwr_mgmt_1 = 0x6B, 00807 // .pwr_mgmt_2 = 0x6C, 00808 // .int_pin_cfg = 0x37, 00809 // .mem_r_w = 0x6F, 00810 // .accel_offs = 0x06, 00811 // .i2c_mst = 0x24, 00812 // .bank_sel = 0x6D, 00813 // .mem_start_addr = 0x6E, 00814 // .prgm_start_h = 0x70 00815 //#ifdef AK89xx_SECONDARY 00816 // ,.raw_compass = 0x49, 00817 // .yg_offs_tc = 0x01, 00818 // .s0_addr = 0x25, 00819 // .s0_reg = 0x26, 00820 // .s0_ctrl = 0x27, 00821 // .s1_addr = 0x28, 00822 // .s1_reg = 0x29, 00823 // .s1_ctrl = 0x2A, 00824 // .s4_ctrl = 0x34, 00825 // .s0_do = 0x63, 00826 // .s1_do = 0x64, 00827 // .i2c_delay_ctrl = 0x67 00828 //#endif 00829 //}; 00830 const struct gyro_reg_s reg = { 00831 0x75, //who_am_i 00832 0x19, //rate_div 00833 0x1A, //lpf 00834 0x0C, //prod_id 00835 0x6A, //user_ctrl 00836 0x23, //fifo_en 00837 0x1B, //gyro_cfg 00838 0x1C, //accel_cfg 00839 0x1F, // motion_thr 00840 0x20, // motion_dur 00841 0x72, // fifo_count_h 00842 0x74, // fifo_r_w 00843 0x43, // raw_gyro 00844 0x3B, // raw_accel 00845 0x41, // temp 00846 0x38, // int_enable 00847 0x39, // dmp_int_status 00848 0x3A, // int_status 00849 0x6B, // pwr_mgmt_1 00850 0x6C, // pwr_mgmt_2 00851 0x37, // int_pin_cfg 00852 0x6F, // mem_r_w 00853 0x06, // accel_offs 00854 0x24, // i2c_mst 00855 0x6D, // bank_sel 00856 0x6E, // mem_start_addr 00857 0x70 // prgm_start_h 00858 }; 00859 00860 //const struct hw_s hw = { 00861 // .addr = 0x68, 00862 // .max_fifo = 1024, 00863 // .num_reg = 118, 00864 // .temp_sens = 340, 00865 // .temp_offset = -521, 00866 // .bank_size = 256 00867 //#if defined AK89xx_SECONDARY 00868 // ,.compass_fsr = AK89xx_FSR 00869 //#endif 00870 //}; 00871 const struct hw_s hw={ 00872 0x68, //addr 00873 1024, //max_fifo 00874 118, //num_reg 00875 340, //temp_sens 00876 -521, //temp_offset 00877 256 //bank_size 00878 }; 00879 00880 //const struct test_s test = { 00881 // .gyro_sens = 32768/250, 00882 // .accel_sens = 32768/16, 00883 // .reg_rate_div = 0, /* 1kHz. */ 00884 // .reg_lpf = 1, /* 188Hz. */ 00885 // .reg_gyro_fsr = 0, /* 250dps. */ 00886 // .reg_accel_fsr = 0x18, /* 16g. */ 00887 // .wait_ms = 50, 00888 // .packet_thresh = 5, /* 5% */ 00889 // .min_dps = 10.f, 00890 // .max_dps = 105.f, 00891 // .max_gyro_var = 0.14f, 00892 // .min_g = 0.3f, 00893 // .max_g = 0.95f, 00894 // .max_accel_var = 0.14f 00895 //}; 00896 const struct test_s test={ 00897 32768/250, //gyro_sens 00898 32768/16, // accel_sens 00899 0, // reg_rate_div 00900 1, // reg_lpf 00901 0, // reg_gyro_fsr 00902 0x18, // reg_accel_fsr 00903 50, // wait_ms 00904 5, // packet_thresh 00905 10.0f, // min_dps 00906 105.0f, // max_dps 00907 0.14f, // max_gyro_var 00908 0.3f, // min_g 00909 0.95f, // max_g 00910 0.14f // max_accel_var 00911 }; 00912 00913 //static struct gyro_state_s st = { 00914 // .reg = ®, 00915 // .hw = &hw, 00916 // .test = &test 00917 //}; 00918 static struct gyro_state_s st={ 00919 ®, 00920 &hw, 00921 {0}, 00922 &test 00923 }; 00924 00925 00926 #elif defined MPU6500 00927 const struct gyro_reg_s reg = { 00928 .who_am_i = 0x75, 00929 .rate_div = 0x19, 00930 .lpf = 0x1A, 00931 .prod_id = 0x0C, 00932 .user_ctrl = 0x6A, 00933 .fifo_en = 0x23, 00934 .gyro_cfg = 0x1B, 00935 .accel_cfg = 0x1C, 00936 .accel_cfg2 = 0x1D, 00937 .lp_accel_odr = 0x1E, 00938 .motion_thr = 0x1F, 00939 .motion_dur = 0x20, 00940 .fifo_count_h = 0x72, 00941 .fifo_r_w = 0x74, 00942 .raw_gyro = 0x43, 00943 .raw_accel = 0x3B, 00944 .temp = 0x41, 00945 .int_enable = 0x38, 00946 .dmp_int_status = 0x39, 00947 .int_status = 0x3A, 00948 .accel_intel = 0x69, 00949 .pwr_mgmt_1 = 0x6B, 00950 .pwr_mgmt_2 = 0x6C, 00951 .int_pin_cfg = 0x37, 00952 .mem_r_w = 0x6F, 00953 .accel_offs = 0x77, 00954 .i2c_mst = 0x24, 00955 .bank_sel = 0x6D, 00956 .mem_start_addr = 0x6E, 00957 .prgm_start_h = 0x70 00958 #ifdef AK89xx_SECONDARY 00959 ,.raw_compass = 0x49, 00960 .s0_addr = 0x25, 00961 .s0_reg = 0x26, 00962 .s0_ctrl = 0x27, 00963 .s1_addr = 0x28, 00964 .s1_reg = 0x29, 00965 .s1_ctrl = 0x2A, 00966 .s4_ctrl = 0x34, 00967 .s0_do = 0x63, 00968 .s1_do = 0x64, 00969 .i2c_delay_ctrl = 0x67 00970 #endif 00971 }; 00972 const struct hw_s hw = { 00973 .addr = 0x68, 00974 .max_fifo = 1024, 00975 .num_reg = 128, 00976 .temp_sens = 321, 00977 .temp_offset = 0, 00978 .bank_size = 256 00979 #if defined AK89xx_SECONDARY 00980 ,.compass_fsr = AK89xx_FSR 00981 #endif 00982 }; 00983 00984 const struct test_s test = { 00985 .gyro_sens = 32768/250, 00986 .accel_sens = 32768/16, 00987 .reg_rate_div = 0, /* 1kHz. */ 00988 .reg_lpf = 1, /* 188Hz. */ 00989 .reg_gyro_fsr = 0, /* 250dps. */ 00990 .reg_accel_fsr = 0x18, /* 16g. */ 00991 .wait_ms = 50, 00992 .packet_thresh = 5, /* 5% */ 00993 .min_dps = 10.f, 00994 .max_dps = 105.f, 00995 .max_gyro_var = 0.14f, 00996 .min_g = 0.3f, 00997 .max_g = 0.95f, 00998 .max_accel_var = 0.14f 00999 }; 01000 01001 static struct gyro_state_s st = { 01002 .reg = ®, 01003 .hw = &hw, 01004 .test = &test 01005 }; 01006 #endif 01007 01008 #define MAX_PACKET_LENGTH (12) 01009 01010 #ifdef AK89xx_SECONDARY 01011 static int setup_compass(void); 01012 #define MAX_COMPASS_SAMPLE_RATE (100) 01013 #endif 01014 01015 /** 01016 * @brief Enable/disable data ready interrupt. 01017 * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready 01018 * interrupt is used. 01019 * @param[in] enable 1 to enable interrupt. 01020 * @return 0 if successful. 01021 */ 01022 static int set_int_enable(unsigned char enable) 01023 { 01024 unsigned char tmp; 01025 01026 if (st.chip_cfg.dmp_on) { 01027 if (enable) 01028 tmp = BIT_DMP_INT_EN; 01029 else 01030 tmp = 0x00; 01031 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 01032 return -1; 01033 st.chip_cfg.int_enable = tmp; 01034 } else { 01035 if (!st.chip_cfg.sensors) 01036 return -1; 01037 if (enable && st.chip_cfg.int_enable) 01038 return 0; 01039 if (enable) 01040 tmp = BIT_DATA_RDY_EN; 01041 else 01042 tmp = 0x00; 01043 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) 01044 return -1; 01045 st.chip_cfg.int_enable = tmp; 01046 } 01047 return 0; 01048 } 01049 01050 /** 01051 * @brief Register dump for testing. 01052 * @return 0 if successful. 01053 */ 01054 int mpu_reg_dump(void) 01055 { 01056 unsigned char ii; 01057 unsigned char data; 01058 01059 for (ii = 0; ii < st.hw->num_reg; ii++) { 01060 if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) 01061 continue; 01062 if (i2c_read(st.hw->addr, ii, 1, &data)) 01063 return -1; 01064 //("%#5x: %#5x\r\n", ii, data); 01065 } 01066 return 0; 01067 } 01068 01069 /** 01070 * @brief Read from a single register. 01071 * NOTE: The memory and FIFO read/write registers cannot be accessed. 01072 * @param[in] reg Register address. 01073 * @param[out] data Register data. 01074 * @return 0 if successful. 01075 */ 01076 int mpu_read_reg(unsigned char reg, unsigned char *data) 01077 { 01078 if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) 01079 return -1; 01080 if (reg >= st.hw->num_reg) 01081 return -1; 01082 return i2c_read(st.hw->addr, reg, 1, data); 01083 } 01084 01085 /** 01086 * @brief Initialize hardware. 01087 * Initial configuration:\n 01088 * Gyro FSR: +/- 2000DPS\n 01089 * Accel FSR +/- 2G\n 01090 * DLPF: 42Hz\n 01091 * FIFO rate: 50Hz\n 01092 * Clock source: Gyro PLL\n 01093 * FIFO: Disabled.\n 01094 * Data ready interrupt: Disabled, active low, unlatched. 01095 * @param[in] int_param Platform-specific parameters to interrupt API. 01096 * @return 0 if successful. 01097 */ 01098 int mpu_init(void) 01099 { 01100 unsigned char data[6], rev; 01101 01102 /* Reset device. */ 01103 data[0] = BIT_RESET; 01104 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 01105 return -1; 01106 delay_ms(100); 01107 01108 /* Wake up chip. */ 01109 data[0] = 0x00; 01110 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 01111 return -1; 01112 01113 #if defined MPU6050 01114 /* Check product revision. */ 01115 if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data)) 01116 return -1; 01117 rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) | 01118 (data[1] & 0x01); 01119 01120 if (rev) { 01121 /* Congrats, these parts are better. */ 01122 if (rev == 1) 01123 st.chip_cfg.accel_half = 1; 01124 else if (rev == 2) 01125 st.chip_cfg.accel_half = 0; 01126 else { 01127 //("Unsupported software product rev %d.\n", rev); 01128 return -1; 01129 } 01130 } else { 01131 if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data)) 01132 return -1; 01133 rev = data[0] & 0x0F; 01134 if (!rev) { 01135 //("Product ID read as 0 indicates device is either " 01136 //"incompatible or an MPU3050.\n"); 01137 return -1; 01138 } else if (rev == 4) { 01139 //("Half sensitivity part found.\n"); 01140 st.chip_cfg.accel_half = 1; 01141 } else 01142 st.chip_cfg.accel_half = 0; 01143 } 01144 #elif defined MPU6500 01145 #define MPU6500_MEM_REV_ADDR (0x17) 01146 if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev)) 01147 return -1; 01148 if (rev == 0x1) 01149 st.chip_cfg.accel_half = 0; 01150 else { 01151 //("Unsupported software product rev %d.\n", rev); 01152 return -1; 01153 } 01154 01155 /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the 01156 * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. 01157 */ 01158 data[0] = BIT_FIFO_SIZE_1024 | 0x8; 01159 if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) 01160 return -1; 01161 #endif 01162 01163 /* Set to invalid values to ensure no I2C writes are skipped. */ 01164 st.chip_cfg.sensors = 0xFF; 01165 st.chip_cfg.gyro_fsr = 0xFF; 01166 st.chip_cfg.accel_fsr = 0xFF; 01167 st.chip_cfg.lpf = 0xFF; 01168 st.chip_cfg.sample_rate = 0xFFFF; 01169 st.chip_cfg.fifo_enable = 0xFF; 01170 st.chip_cfg.bypass_mode = 0xFF; 01171 #ifdef AK89xx_SECONDARY 01172 st.chip_cfg.compass_sample_rate = 0xFFFF; 01173 #endif 01174 /* mpu_set_sensors always preserves this setting. */ 01175 st.chip_cfg.clk_src = INV_CLK_PLL; 01176 /* Handled in next call to mpu_set_bypass. */ 01177 st.chip_cfg.active_low_int = 1; 01178 st.chip_cfg.latched_int = 0; 01179 st.chip_cfg.int_motion_only = 0; 01180 st.chip_cfg.lp_accel_mode = 0; 01181 memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); 01182 st.chip_cfg.dmp_on = 0; 01183 st.chip_cfg.dmp_loaded = 0; 01184 st.chip_cfg.dmp_sample_rate = 0; 01185 01186 if (mpu_set_gyro_fsr(2000)) 01187 return -1; 01188 if (mpu_set_accel_fsr(2)) 01189 return -1; 01190 if (mpu_set_lpf(42)) 01191 return -1; 01192 if (mpu_set_sample_rate(50)) 01193 return -1; 01194 if (mpu_configure_fifo(0)) 01195 return -1; 01196 01197 // if (int_param) 01198 // reg_int_cb(int_param); 01199 01200 #ifdef AK89xx_SECONDARY 01201 setup_compass(); 01202 if (mpu_set_compass_sample_rate(10)) 01203 return -1; 01204 #else 01205 /* Already disabled by setup_compass. */ 01206 if (mpu_set_bypass(0)) 01207 return -1; 01208 #endif 01209 01210 mpu_set_sensors(0); 01211 return 0; 01212 } 01213 01214 /** 01215 * @brief Enter low-power accel-only mode. 01216 * In low-power accel mode, the chip goes to sleep and only wakes up to sample 01217 * the accelerometer at one of the following frequencies: 01218 * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz 01219 * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz 01220 * \n If the requested rate is not one listed above, the device will be set to 01221 * the next highest rate. Requesting a rate above the maximum supported 01222 * frequency will result in an error. 01223 * \n To select a fractional wake-up frequency, round down the value passed to 01224 * @e rate. 01225 * @param[in] rate Minimum sampling rate, or zero to disable LP 01226 * accel mode. 01227 * @return 0 if successful. 01228 */ 01229 int mpu_lp_accel_mode(unsigned char rate) 01230 { 01231 unsigned char tmp[2]; 01232 01233 if (rate > 40) 01234 return -1; 01235 01236 if (!rate) { 01237 mpu_set_int_latched(0); 01238 tmp[0] = 0; 01239 tmp[1] = BIT_STBY_XYZG; 01240 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 01241 return -1; 01242 st.chip_cfg.lp_accel_mode = 0; 01243 return 0; 01244 } 01245 /* For LP accel, we automatically configure the hardware to produce latched 01246 * interrupts. In LP accel mode, the hardware cycles into sleep mode before 01247 * it gets a chance to deassert the interrupt pin; therefore, we shift this 01248 * responsibility over to the MCU. 01249 * 01250 * Any register read will clear the interrupt. 01251 */ 01252 mpu_set_int_latched(1); 01253 #if defined MPU6050 01254 tmp[0] = BIT_LPA_CYCLE; 01255 if (rate == 1) { 01256 tmp[1] = INV_LPA_1_25HZ; 01257 mpu_set_lpf(5); 01258 } else if (rate <= 5) { 01259 tmp[1] = INV_LPA_5HZ; 01260 mpu_set_lpf(5); 01261 } else if (rate <= 20) { 01262 tmp[1] = INV_LPA_20HZ; 01263 mpu_set_lpf(10); 01264 } else { 01265 tmp[1] = INV_LPA_40HZ; 01266 mpu_set_lpf(20); 01267 } 01268 tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; 01269 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) 01270 return -1; 01271 #elif defined MPU6500 01272 /* Set wake frequency. */ 01273 if (rate == 1) 01274 tmp[0] = INV_LPA_1_25HZ; 01275 else if (rate == 2) 01276 tmp[0] = INV_LPA_2_5HZ; 01277 else if (rate <= 5) 01278 tmp[0] = INV_LPA_5HZ; 01279 else if (rate <= 10) 01280 tmp[0] = INV_LPA_10HZ; 01281 else if (rate <= 20) 01282 tmp[0] = INV_LPA_20HZ; 01283 else if (rate <= 40) 01284 tmp[0] = INV_LPA_40HZ; 01285 else if (rate <= 80) 01286 tmp[0] = INV_LPA_80HZ; 01287 else if (rate <= 160) 01288 tmp[0] = INV_LPA_160HZ; 01289 else if (rate <= 320) 01290 tmp[0] = INV_LPA_320HZ; 01291 else 01292 tmp[0] = INV_LPA_640HZ; 01293 if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) 01294 return -1; 01295 tmp[0] = BIT_LPA_CYCLE; 01296 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) 01297 return -1; 01298 #endif 01299 st.chip_cfg.sensors = INV_XYZ_ACCEL; 01300 st.chip_cfg.clk_src = 0; 01301 st.chip_cfg.lp_accel_mode = 1; 01302 mpu_configure_fifo(0); 01303 01304 return 0; 01305 } 01306 01307 /** 01308 * @brief Read raw gyro data directly from the registers. 01309 * @param[out] data Raw data in hardware units. 01310 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 01311 * @return 0 if successful. 01312 */ 01313 int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 01314 { 01315 unsigned char tmp[6]; 01316 01317 if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) 01318 return -1; 01319 01320 if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) 01321 return -1; 01322 data[0] = (tmp[0] << 8) | tmp[1]; 01323 data[1] = (tmp[2] << 8) | tmp[3]; 01324 data[2] = (tmp[4] << 8) | tmp[5]; 01325 if (timestamp) 01326 get_ms(timestamp); 01327 return 0; 01328 } 01329 01330 /** 01331 * @brief Read raw accel data directly from the registers. 01332 * @param[out] data Raw data in hardware units. 01333 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 01334 * @return 0 if successful. 01335 */ 01336 int mpu_get_accel_reg(short *data, unsigned long *timestamp) 01337 { 01338 unsigned char tmp[6]; 01339 01340 if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) 01341 return -1; 01342 01343 if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) 01344 return -1; 01345 data[0] = (tmp[0] << 8) | tmp[1]; 01346 data[1] = (tmp[2] << 8) | tmp[3]; 01347 data[2] = (tmp[4] << 8) | tmp[5]; 01348 if (timestamp) 01349 get_ms(timestamp); 01350 return 0; 01351 } 01352 01353 /** 01354 * @brief Read temperature data directly from the registers. 01355 * @param[out] data Data in q16 format. 01356 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 01357 * @return 0 if successful. 01358 */ 01359 int mpu_get_temperature(long *data, unsigned long *timestamp) 01360 { 01361 unsigned char tmp[2]; 01362 short raw; 01363 01364 if (!(st.chip_cfg.sensors)) 01365 return -1; 01366 01367 if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp)) 01368 return -1; 01369 raw = (tmp[0] << 8) | tmp[1]; 01370 if (timestamp) 01371 get_ms(timestamp); 01372 01373 data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); 01374 return 0; 01375 } 01376 01377 /** 01378 * @brief Push biases to the accel bias registers. 01379 * This function expects biases relative to the current sensor output, and 01380 * these biases will be added to the factory-supplied values. 01381 * @param[in] accel_bias New biases. 01382 * @return 0 if successful. 01383 */ 01384 int mpu_set_accel_bias(const long *accel_bias) 01385 { 01386 unsigned char data[6]; 01387 short accel_hw[3]; 01388 short got_accel[3]; 01389 short fg[3]; 01390 01391 if (!accel_bias) 01392 return -1; 01393 if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2]) 01394 return 0; 01395 01396 if (i2c_read(st.hw->addr, 3, 3, data)) 01397 return -1; 01398 fg[0] = ((data[0] >> 4) + 8) & 0xf; 01399 fg[1] = ((data[1] >> 4) + 8) & 0xf; 01400 fg[2] = ((data[2] >> 4) + 8) & 0xf; 01401 01402 accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0])); 01403 accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1])); 01404 accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2])); 01405 01406 if (i2c_read(st.hw->addr, 0x06, 6, data)) 01407 return -1; 01408 01409 got_accel[0] = ((short)data[0] << 8) | data[1]; 01410 got_accel[1] = ((short)data[2] << 8) | data[3]; 01411 got_accel[2] = ((short)data[4] << 8) | data[5]; 01412 01413 accel_hw[0] += got_accel[0]; 01414 accel_hw[1] += got_accel[1]; 01415 accel_hw[2] += got_accel[2]; 01416 01417 data[0] = (accel_hw[0] >> 8) & 0xff; 01418 data[1] = (accel_hw[0]) & 0xff; 01419 data[2] = (accel_hw[1] >> 8) & 0xff; 01420 data[3] = (accel_hw[1]) & 0xff; 01421 data[4] = (accel_hw[2] >> 8) & 0xff; 01422 data[5] = (accel_hw[2]) & 0xff; 01423 01424 if (i2c_write(st.hw->addr, 0x06, 6, data)) 01425 return -1; 01426 return 0; 01427 } 01428 01429 /** 01430 * @brief Reset FIFO read/write pointers. 01431 * @return 0 if successful. 01432 */ 01433 int mpu_reset_fifo(void) 01434 { 01435 unsigned char data; 01436 01437 if (!(st.chip_cfg.sensors)) 01438 return -1; 01439 01440 data = 0; 01441 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01442 return -1; 01443 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01444 return -1; 01445 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01446 return -1; 01447 01448 if (st.chip_cfg.dmp_on) { 01449 data = BIT_FIFO_RST | BIT_DMP_RST; 01450 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01451 return -1; 01452 delay_ms(50); 01453 data = BIT_DMP_EN | BIT_FIFO_EN; 01454 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 01455 data |= BIT_AUX_IF_EN; 01456 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01457 return -1; 01458 if (st.chip_cfg.int_enable) 01459 data = BIT_DMP_INT_EN; 01460 else 01461 data = 0; 01462 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01463 return -1; 01464 data = 0; 01465 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) 01466 return -1; 01467 } else { 01468 data = BIT_FIFO_RST; 01469 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01470 return -1; 01471 if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 01472 data = BIT_FIFO_EN; 01473 else 01474 data = BIT_FIFO_EN | BIT_AUX_IF_EN; 01475 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) 01476 return -1; 01477 delay_ms(50); 01478 if (st.chip_cfg.int_enable) 01479 data = BIT_DATA_RDY_EN; 01480 else 01481 data = 0; 01482 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) 01483 return -1; 01484 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) 01485 return -1; 01486 } 01487 return 0; 01488 } 01489 01490 /** 01491 * @brief Get the gyro full-scale range. 01492 * @param[out] fsr Current full-scale range. 01493 * @return 0 if successful. 01494 */ 01495 int mpu_get_gyro_fsr(unsigned short *fsr) 01496 { 01497 switch (st.chip_cfg.gyro_fsr) { 01498 case INV_FSR_250DPS: 01499 fsr[0] = 250; 01500 break; 01501 case INV_FSR_500DPS: 01502 fsr[0] = 500; 01503 break; 01504 case INV_FSR_1000DPS: 01505 fsr[0] = 1000; 01506 break; 01507 case INV_FSR_2000DPS: 01508 fsr[0] = 2000; 01509 break; 01510 default: 01511 fsr[0] = 0; 01512 break; 01513 } 01514 return 0; 01515 } 01516 01517 /** 01518 * @brief Set the gyro full-scale range. 01519 * @param[in] fsr Desired full-scale range. 01520 * @return 0 if successful. 01521 */ 01522 int mpu_set_gyro_fsr(unsigned short fsr) 01523 { 01524 unsigned char data; 01525 01526 if (!(st.chip_cfg.sensors)) 01527 return -1; 01528 01529 switch (fsr) { 01530 case 250: 01531 data = INV_FSR_250DPS << 3; 01532 break; 01533 case 500: 01534 data = INV_FSR_500DPS << 3; 01535 break; 01536 case 1000: 01537 data = INV_FSR_1000DPS << 3; 01538 break; 01539 case 2000: 01540 data = INV_FSR_2000DPS << 3; 01541 break; 01542 default: 01543 return -1; 01544 } 01545 01546 if (st.chip_cfg.gyro_fsr == (data >> 3)) 01547 return 0; 01548 if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) 01549 return -1; 01550 st.chip_cfg.gyro_fsr = data >> 3; 01551 return 0; 01552 } 01553 01554 /** 01555 * @brief Get the accel full-scale range. 01556 * @param[out] fsr Current full-scale range. 01557 * @return 0 if successful. 01558 */ 01559 int mpu_get_accel_fsr(unsigned char *fsr) 01560 { 01561 switch (st.chip_cfg.accel_fsr) { 01562 case INV_FSR_2G: 01563 fsr[0] = 2; 01564 break; 01565 case INV_FSR_4G: 01566 fsr[0] = 4; 01567 break; 01568 case INV_FSR_8G: 01569 fsr[0] = 8; 01570 break; 01571 case INV_FSR_16G: 01572 fsr[0] = 16; 01573 break; 01574 default: 01575 return -1; 01576 } 01577 if (st.chip_cfg.accel_half) 01578 fsr[0] <<= 1; 01579 return 0; 01580 } 01581 01582 /** 01583 * @brief Set the accel full-scale range. 01584 * @param[in] fsr Desired full-scale range. 01585 * @return 0 if successful. 01586 */ 01587 int mpu_set_accel_fsr(unsigned char fsr) 01588 { 01589 unsigned char data; 01590 01591 if (!(st.chip_cfg.sensors)) 01592 return -1; 01593 01594 switch (fsr) { 01595 case 2: 01596 data = INV_FSR_2G << 3; 01597 break; 01598 case 4: 01599 data = INV_FSR_4G << 3; 01600 break; 01601 case 8: 01602 data = INV_FSR_8G << 3; 01603 break; 01604 case 16: 01605 data = INV_FSR_16G << 3; 01606 break; 01607 default: 01608 return -1; 01609 } 01610 01611 if (st.chip_cfg.accel_fsr == (data >> 3)) 01612 return 0; 01613 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) 01614 return -1; 01615 st.chip_cfg.accel_fsr = data >> 3; 01616 return 0; 01617 } 01618 01619 /** 01620 * @brief Get the current DLPF setting. 01621 * @param[out] lpf Current LPF setting. 01622 * 0 if successful. 01623 */ 01624 int mpu_get_lpf(unsigned short *lpf) 01625 { 01626 switch (st.chip_cfg.lpf) { 01627 case INV_FILTER_188HZ: 01628 lpf[0] = 188; 01629 break; 01630 case INV_FILTER_98HZ: 01631 lpf[0] = 98; 01632 break; 01633 case INV_FILTER_42HZ: 01634 lpf[0] = 42; 01635 break; 01636 case INV_FILTER_20HZ: 01637 lpf[0] = 20; 01638 break; 01639 case INV_FILTER_10HZ: 01640 lpf[0] = 10; 01641 break; 01642 case INV_FILTER_5HZ: 01643 lpf[0] = 5; 01644 break; 01645 case INV_FILTER_256HZ_NOLPF2: 01646 case INV_FILTER_2100HZ_NOLPF: 01647 default: 01648 lpf[0] = 0; 01649 break; 01650 } 01651 return 0; 01652 } 01653 01654 /** 01655 * @brief Set digital low pass filter. 01656 * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. 01657 * @param[in] lpf Desired LPF setting. 01658 * @return 0 if successful. 01659 */ 01660 int mpu_set_lpf(unsigned short lpf) 01661 { 01662 unsigned char data; 01663 01664 if (!(st.chip_cfg.sensors)) 01665 return -1; 01666 01667 if (lpf >= 188) 01668 data = INV_FILTER_188HZ; 01669 else if (lpf >= 98) 01670 data = INV_FILTER_98HZ; 01671 else if (lpf >= 42) 01672 data = INV_FILTER_42HZ; 01673 else if (lpf >= 20) 01674 data = INV_FILTER_20HZ; 01675 else if (lpf >= 10) 01676 data = INV_FILTER_10HZ; 01677 else 01678 data = INV_FILTER_5HZ; 01679 01680 if (st.chip_cfg.lpf == data) 01681 return 0; 01682 if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data)) 01683 return -1; 01684 st.chip_cfg.lpf = data; 01685 return 0; 01686 } 01687 01688 /** 01689 * @brief Get sampling rate. 01690 * @param[out] rate Current sampling rate (Hz). 01691 * @return 0 if successful. 01692 */ 01693 int mpu_get_sample_rate(unsigned short *rate) 01694 { 01695 if (st.chip_cfg.dmp_on) 01696 return -1; 01697 else 01698 rate[0] = st.chip_cfg.sample_rate; 01699 return 0; 01700 } 01701 01702 /** 01703 * @brief Set sampling rate. 01704 * Sampling rate must be between 4Hz and 1kHz. 01705 * @param[in] rate Desired sampling rate (Hz). 01706 * @return 0 if successful. 01707 */ 01708 int mpu_set_sample_rate(unsigned short rate) 01709 { 01710 unsigned char data; 01711 01712 if (!(st.chip_cfg.sensors)) 01713 return -1; 01714 01715 if (st.chip_cfg.dmp_on) 01716 return -1; 01717 else { 01718 if (st.chip_cfg.lp_accel_mode) { 01719 if (rate && (rate <= 40)) { 01720 /* Just stay in low-power accel mode. */ 01721 mpu_lp_accel_mode(rate); 01722 return 0; 01723 } 01724 /* Requested rate exceeds the allowed frequencies in LP accel mode, 01725 * switch back to full-power mode. 01726 */ 01727 mpu_lp_accel_mode(0); 01728 } 01729 if (rate < 4) 01730 rate = 4; 01731 else if (rate > 1000) 01732 rate = 1000; 01733 01734 data = 1000 / rate - 1; 01735 if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data)) 01736 return -1; 01737 01738 st.chip_cfg.sample_rate = 1000 / (1 + data); 01739 01740 #ifdef AK89xx_SECONDARY 01741 mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); 01742 #endif 01743 01744 /* Automatically set LPF to 1/2 sampling rate. */ 01745 mpu_set_lpf(st.chip_cfg.sample_rate >> 1); 01746 return 0; 01747 } 01748 } 01749 01750 /** 01751 * @brief Get compass sampling rate. 01752 * @param[out] rate Current compass sampling rate (Hz). 01753 * @return 0 if successful. 01754 */ 01755 int mpu_get_compass_sample_rate(unsigned short *rate) 01756 { 01757 #ifdef AK89xx_SECONDARY 01758 rate[0] = st.chip_cfg.compass_sample_rate; 01759 return 0; 01760 #else 01761 rate[0] = 0; 01762 return -1; 01763 #endif 01764 } 01765 01766 /** 01767 * @brief Set compass sampling rate. 01768 * The compass on the auxiliary I2C bus is read by the MPU hardware at a 01769 * maximum of 100Hz. The actual rate can be set to a fraction of the gyro 01770 * sampling rate. 01771 * 01772 * \n WARNING: The new rate may be different than what was requested. Call 01773 * mpu_get_compass_sample_rate to check the actual setting. 01774 * @param[in] rate Desired compass sampling rate (Hz). 01775 * @return 0 if successful. 01776 */ 01777 int mpu_set_compass_sample_rate(unsigned short rate) 01778 { 01779 #ifdef AK89xx_SECONDARY 01780 unsigned char div; 01781 if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) 01782 return -1; 01783 01784 div = st.chip_cfg.sample_rate / rate - 1; 01785 if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) 01786 return -1; 01787 st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); 01788 return 0; 01789 #else 01790 return -1; 01791 #endif 01792 } 01793 01794 /** 01795 * @brief Get gyro sensitivity scale factor. 01796 * @param[out] sens Conversion from hardware units to dps. 01797 * @return 0 if successful. 01798 */ 01799 int mpu_get_gyro_sens(float *sens) 01800 { 01801 switch (st.chip_cfg.gyro_fsr) { 01802 case INV_FSR_250DPS: 01803 sens[0] = 131.f; 01804 break; 01805 case INV_FSR_500DPS: 01806 sens[0] = 65.5f; 01807 break; 01808 case INV_FSR_1000DPS: 01809 sens[0] = 32.8f; 01810 break; 01811 case INV_FSR_2000DPS: 01812 sens[0] = 16.4f; 01813 break; 01814 default: 01815 return -1; 01816 } 01817 return 0; 01818 } 01819 01820 /** 01821 * @brief Get accel sensitivity scale factor. 01822 * @param[out] sens Conversion from hardware units to g's. 01823 * @return 0 if successful. 01824 */ 01825 int mpu_get_accel_sens(unsigned short *sens) 01826 { 01827 switch (st.chip_cfg.accel_fsr) { 01828 case INV_FSR_2G: 01829 sens[0] = 16384; 01830 break; 01831 case INV_FSR_4G: 01832 sens[0] = 8092; 01833 break; 01834 case INV_FSR_8G: 01835 sens[0] = 4096; 01836 break; 01837 case INV_FSR_16G: 01838 sens[0] = 2048; 01839 break; 01840 default: 01841 return -1; 01842 } 01843 if (st.chip_cfg.accel_half) 01844 sens[0] >>= 1; 01845 return 0; 01846 } 01847 01848 /** 01849 * @brief Get current FIFO configuration. 01850 * @e sensors can contain a combination of the following flags: 01851 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01852 * \n INV_XYZ_GYRO 01853 * \n INV_XYZ_ACCEL 01854 * @param[out] sensors Mask of sensors in FIFO. 01855 * @return 0 if successful. 01856 */ 01857 int mpu_get_fifo_config(unsigned char *sensors) 01858 { 01859 sensors[0] = st.chip_cfg.fifo_enable; 01860 return 0; 01861 } 01862 01863 /** 01864 * @brief Select which sensors are pushed to FIFO. 01865 * @e sensors can contain a combination of the following flags: 01866 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01867 * \n INV_XYZ_GYRO 01868 * \n INV_XYZ_ACCEL 01869 * @param[in] sensors Mask of sensors to push to FIFO. 01870 * @return 0 if successful. 01871 */ 01872 int mpu_configure_fifo(unsigned char sensors) 01873 { 01874 unsigned char prev; 01875 int result = 0; 01876 01877 /* Compass data isn't going into the FIFO. Stop trying. */ 01878 sensors &= ~INV_XYZ_COMPASS; 01879 01880 if (st.chip_cfg.dmp_on) 01881 return 0; 01882 else { 01883 if (!(st.chip_cfg.sensors)) 01884 return -1; 01885 prev = st.chip_cfg.fifo_enable; 01886 st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; 01887 if (st.chip_cfg.fifo_enable != sensors) 01888 /* You're not getting what you asked for. Some sensors are 01889 * asleep. 01890 */ 01891 result = -1; 01892 else 01893 result = 0; 01894 if (sensors || st.chip_cfg.lp_accel_mode) 01895 set_int_enable(1); 01896 else 01897 set_int_enable(0); 01898 if (sensors) { 01899 if (mpu_reset_fifo()) { 01900 st.chip_cfg.fifo_enable = prev; 01901 return -1; 01902 } 01903 } 01904 } 01905 01906 return result; 01907 } 01908 01909 /** 01910 * @brief Get current power state. 01911 * @param[in] power_on 1 if turned on, 0 if suspended. 01912 * @return 0 if successful. 01913 */ 01914 int mpu_get_power_state(unsigned char *power_on) 01915 { 01916 if (st.chip_cfg.sensors) 01917 power_on[0] = 1; 01918 else 01919 power_on[0] = 0; 01920 return 0; 01921 } 01922 01923 /** 01924 * @brief Turn specific sensors on/off. 01925 * @e sensors can contain a combination of the following flags: 01926 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01927 * \n INV_XYZ_GYRO 01928 * \n INV_XYZ_ACCEL 01929 * \n INV_XYZ_COMPASS 01930 * @param[in] sensors Mask of sensors to wake. 01931 * @return 0 if successful. 01932 */ 01933 int mpu_set_sensors(unsigned char sensors) 01934 { 01935 unsigned char data; 01936 #ifdef AK89xx_SECONDARY 01937 unsigned char user_ctrl; 01938 #endif 01939 01940 if (sensors & INV_XYZ_GYRO) 01941 data = INV_CLK_PLL; 01942 else if (sensors) 01943 data = 0; 01944 else 01945 data = BIT_SLEEP; 01946 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { 01947 st.chip_cfg.sensors = 0; 01948 return -1; 01949 } 01950 st.chip_cfg.clk_src = data & ~BIT_SLEEP; 01951 01952 data = 0; 01953 if (!(sensors & INV_X_GYRO)) 01954 data |= BIT_STBY_XG; 01955 if (!(sensors & INV_Y_GYRO)) 01956 data |= BIT_STBY_YG; 01957 if (!(sensors & INV_Z_GYRO)) 01958 data |= BIT_STBY_ZG; 01959 if (!(sensors & INV_XYZ_ACCEL)) 01960 data |= BIT_STBY_XYZA; 01961 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { 01962 st.chip_cfg.sensors = 0; 01963 return -1; 01964 } 01965 01966 if (sensors && (sensors != INV_XYZ_ACCEL)) 01967 /* Latched interrupts only used in LP accel mode. */ 01968 mpu_set_int_latched(0); 01969 01970 #ifdef AK89xx_SECONDARY 01971 #ifdef AK89xx_BYPASS 01972 if (sensors & INV_XYZ_COMPASS) 01973 mpu_set_bypass(1); 01974 else 01975 mpu_set_bypass(0); 01976 #else 01977 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01978 return -1; 01979 /* Handle AKM power management. */ 01980 if (sensors & INV_XYZ_COMPASS) { 01981 data = AKM_SINGLE_MEASUREMENT; 01982 user_ctrl |= BIT_AUX_IF_EN; 01983 } else { 01984 data = AKM_POWER_DOWN; 01985 user_ctrl &= ~BIT_AUX_IF_EN; 01986 } 01987 if (st.chip_cfg.dmp_on) 01988 user_ctrl |= BIT_DMP_EN; 01989 else 01990 user_ctrl &= ~BIT_DMP_EN; 01991 if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) 01992 return -1; 01993 /* Enable/disable I2C master mode. */ 01994 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) 01995 return -1; 01996 #endif 01997 #endif 01998 01999 st.chip_cfg.sensors = sensors; 02000 st.chip_cfg.lp_accel_mode = 0; 02001 delay_ms(50); 02002 return 0; 02003 } 02004 02005 /** 02006 * @brief Read the MPU interrupt status registers. 02007 * @param[out] status Mask of interrupt bits. 02008 * @return 0 if successful. 02009 */ 02010 int mpu_get_int_status(short *status) 02011 { 02012 unsigned char tmp[2]; 02013 if (!st.chip_cfg.sensors) 02014 return -1; 02015 if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) 02016 return -1; 02017 status[0] = (tmp[0] << 8) | tmp[1]; 02018 return 0; 02019 } 02020 02021 /** 02022 * @brief Get one packet from the FIFO. 02023 * If @e sensors does not contain a particular sensor, disregard the data 02024 * returned to that pointer. 02025 * \n @e sensors can contain a combination of the following flags: 02026 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 02027 * \n INV_XYZ_GYRO 02028 * \n INV_XYZ_ACCEL 02029 * \n If the FIFO has no new data, @e sensors will be zero. 02030 * \n If the FIFO is disabled, @e sensors will be zero and this function will 02031 * return a non-zero error code. 02032 * @param[out] gyro Gyro data in hardware units. 02033 * @param[out] accel Accel data in hardware units. 02034 * @param[out] timestamp Timestamp in milliseconds. 02035 * @param[out] sensors Mask of sensors read from FIFO. 02036 * @param[out] more Number of remaining packets. 02037 * @return 0 if successful. 02038 */ 02039 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, 02040 unsigned char *sensors, unsigned char *more) 02041 { 02042 /* Assumes maximum packet size is gyro (6) + accel (6). */ 02043 unsigned char data[MAX_PACKET_LENGTH]; 02044 unsigned char packet_size = 0; 02045 unsigned short fifo_count, index = 0; 02046 02047 if (st.chip_cfg.dmp_on) 02048 return -1; 02049 02050 sensors[0] = 0; 02051 if (!st.chip_cfg.sensors) 02052 return -1; 02053 if (!st.chip_cfg.fifo_enable) 02054 return -1; 02055 02056 if (st.chip_cfg.fifo_enable & INV_X_GYRO) 02057 packet_size += 2; 02058 if (st.chip_cfg.fifo_enable & INV_Y_GYRO) 02059 packet_size += 2; 02060 if (st.chip_cfg.fifo_enable & INV_Z_GYRO) 02061 packet_size += 2; 02062 if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) 02063 packet_size += 6; 02064 02065 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 02066 return -1; 02067 fifo_count = (data[0] << 8) | data[1]; 02068 if (fifo_count < packet_size) 02069 return 0; 02070 // //("FIFO count: %hd\n", fifo_count); 02071 if (fifo_count > (st.hw->max_fifo >> 1)) { 02072 /* FIFO is 50% full, better check overflow bit. */ 02073 if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) 02074 return -1; 02075 if (data[0] & BIT_FIFO_OVERFLOW) { 02076 mpu_reset_fifo(); 02077 return -2; 02078 } 02079 } 02080 get_ms((unsigned long*)timestamp); 02081 02082 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) 02083 return -1; 02084 more[0] = fifo_count / packet_size - 1; 02085 sensors[0] = 0; 02086 02087 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { 02088 accel[0] = (data[index+0] << 8) | data[index+1]; 02089 accel[1] = (data[index+2] << 8) | data[index+3]; 02090 accel[2] = (data[index+4] << 8) | data[index+5]; 02091 sensors[0] |= INV_XYZ_ACCEL; 02092 index += 6; 02093 } 02094 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { 02095 gyro[0] = (data[index+0] << 8) | data[index+1]; 02096 sensors[0] |= INV_X_GYRO; 02097 index += 2; 02098 } 02099 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { 02100 gyro[1] = (data[index+0] << 8) | data[index+1]; 02101 sensors[0] |= INV_Y_GYRO; 02102 index += 2; 02103 } 02104 if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { 02105 gyro[2] = (data[index+0] << 8) | data[index+1]; 02106 sensors[0] |= INV_Z_GYRO; 02107 index += 2; 02108 } 02109 02110 return 0; 02111 } 02112 02113 /** 02114 * @brief Get one unparsed packet from the FIFO. 02115 * This function should be used if the packet is to be parsed elsewhere. 02116 * @param[in] length Length of one FIFO packet. 02117 * @param[in] data FIFO packet. 02118 * @param[in] more Number of remaining packets. 02119 */ 02120 int mpu_read_fifo_stream(unsigned short length, unsigned char *data, 02121 unsigned char *more) 02122 { 02123 unsigned char tmp[2]; 02124 unsigned short fifo_count; 02125 if (!st.chip_cfg.dmp_on) 02126 return -1; 02127 if (!st.chip_cfg.sensors) 02128 return -1; 02129 02130 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) 02131 return -1; 02132 fifo_count = (tmp[0] << 8) | tmp[1]; 02133 if (fifo_count < length) { 02134 more[0] = 0; 02135 return -1; 02136 } 02137 if (fifo_count > (st.hw->max_fifo >> 1)) { 02138 /* FIFO is 50% full, better check overflow bit. */ 02139 if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp)) 02140 return -1; 02141 if (tmp[0] & BIT_FIFO_OVERFLOW) { 02142 mpu_reset_fifo(); 02143 return -2; 02144 } 02145 } 02146 02147 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data)) 02148 return -1; 02149 more[0] = fifo_count / length - 1; 02150 return 0; 02151 } 02152 02153 /** 02154 * @brief Set device to bypass mode. 02155 * @param[in] bypass_on 1 to enable bypass mode. 02156 * @return 0 if successful. 02157 */ 02158 int mpu_set_bypass(unsigned char bypass_on) 02159 { 02160 unsigned char tmp; 02161 02162 if (st.chip_cfg.bypass_mode == bypass_on) 02163 return 0; 02164 02165 if (bypass_on) { 02166 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 02167 return -1; 02168 tmp &= ~BIT_AUX_IF_EN; 02169 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 02170 return -1; 02171 delay_ms(3); 02172 tmp = BIT_BYPASS_EN; 02173 if (st.chip_cfg.active_low_int) 02174 tmp |= BIT_ACTL; 02175 if (st.chip_cfg.latched_int) 02176 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 02177 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 02178 return -1; 02179 } else { 02180 /* Enable I2C master mode if compass is being used. */ 02181 if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 02182 return -1; 02183 if (st.chip_cfg.sensors & INV_XYZ_COMPASS) 02184 tmp |= BIT_AUX_IF_EN; 02185 else 02186 tmp &= ~BIT_AUX_IF_EN; 02187 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) 02188 return -1; 02189 delay_ms(3); 02190 if (st.chip_cfg.active_low_int) 02191 tmp = BIT_ACTL; 02192 else 02193 tmp = 0; 02194 if (st.chip_cfg.latched_int) 02195 tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; 02196 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 02197 return -1; 02198 } 02199 st.chip_cfg.bypass_mode = bypass_on; 02200 return 0; 02201 } 02202 02203 /** 02204 * @brief Set interrupt level. 02205 * @param[in] active_low 1 for active low, 0 for active high. 02206 * @return 0 if successful. 02207 */ 02208 int mpu_set_int_level(unsigned char active_low) 02209 { 02210 st.chip_cfg.active_low_int = active_low; 02211 return 0; 02212 } 02213 02214 /** 02215 * @brief Enable latched interrupts. 02216 * Any MPU register will clear the interrupt. 02217 * @param[in] enable 1 to enable, 0 to disable. 02218 * @return 0 if successful. 02219 */ 02220 int mpu_set_int_latched(unsigned char enable) 02221 { 02222 unsigned char tmp; 02223 if (st.chip_cfg.latched_int == enable) 02224 return 0; 02225 02226 if (enable) 02227 tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; 02228 else 02229 tmp = 0; 02230 if (st.chip_cfg.bypass_mode) 02231 tmp |= BIT_BYPASS_EN; 02232 if (st.chip_cfg.active_low_int) 02233 tmp |= BIT_ACTL; 02234 if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) 02235 return -1; 02236 st.chip_cfg.latched_int = enable; 02237 return 0; 02238 } 02239 02240 #ifdef MPU6050 02241 static int get_accel_prod_shift(float *st_shift) 02242 { 02243 unsigned char tmp[4], shift_code[3], ii; 02244 02245 if (i2c_read(st.hw->addr, 0x0D, 4, tmp)) 02246 return 0x07; 02247 02248 shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); 02249 shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); 02250 shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); 02251 for (ii = 0; ii < 3; ii++) { 02252 if (!shift_code[ii]) { 02253 st_shift[ii] = 0.f; 02254 continue; 02255 } 02256 /* Equivalent to.. 02257 * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) 02258 */ 02259 st_shift[ii] = 0.34f; 02260 while (--shift_code[ii]) 02261 st_shift[ii] *= 1.034f; 02262 } 02263 return 0; 02264 } 02265 02266 static int accel_self_test(long *bias_regular, long *bias_st) 02267 { 02268 int jj, result = 0; 02269 float st_shift[3], st_shift_cust, st_shift_var; 02270 02271 get_accel_prod_shift(st_shift); 02272 for(jj = 0; jj < 3; jj++) { 02273 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 02274 if (st_shift[jj]) { 02275 st_shift_var = st_shift_cust / st_shift[jj] - 1.f; 02276 if (fabs(st_shift_var) > test.max_accel_var) 02277 result |= 1 << jj; 02278 } else if ((st_shift_cust < test.min_g) || 02279 (st_shift_cust > test.max_g)) 02280 result |= 1 << jj; 02281 } 02282 02283 return result; 02284 } 02285 02286 static int gyro_self_test(long *bias_regular, long *bias_st) 02287 { 02288 int jj, result = 0; 02289 unsigned char tmp[3]; 02290 float st_shift, st_shift_cust, st_shift_var; 02291 02292 if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) 02293 return 0x07; 02294 02295 tmp[0] &= 0x1F; 02296 tmp[1] &= 0x1F; 02297 tmp[2] &= 0x1F; 02298 02299 for (jj = 0; jj < 3; jj++) { 02300 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; 02301 if (tmp[jj]) { 02302 st_shift = 3275.f / test.gyro_sens; 02303 while (--tmp[jj]) 02304 st_shift *= 1.046f; 02305 st_shift_var = st_shift_cust / st_shift - 1.f; 02306 if (fabs(st_shift_var) > test.max_gyro_var) 02307 result |= 1 << jj; 02308 } else if ((st_shift_cust < test.min_dps) || 02309 (st_shift_cust > test.max_dps)) 02310 result |= 1 << jj; 02311 } 02312 return result; 02313 } 02314 02315 #ifdef AK89xx_SECONDARY 02316 static int compass_self_test(void) 02317 { 02318 unsigned char tmp[6]; 02319 unsigned char tries = 10; 02320 int result = 0x07; 02321 short data; 02322 02323 mpu_set_bypass(1); 02324 02325 tmp[0] = AKM_POWER_DOWN; 02326 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 02327 return 0x07; 02328 tmp[0] = AKM_BIT_SELF_TEST; 02329 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) 02330 goto AKM_restore; 02331 tmp[0] = AKM_MODE_SELF_TEST; 02332 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) 02333 goto AKM_restore; 02334 02335 do { 02336 delay_ms(10); 02337 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) 02338 goto AKM_restore; 02339 if (tmp[0] & AKM_DATA_READY) 02340 break; 02341 } while (tries--); 02342 if (!(tmp[0] & AKM_DATA_READY)) 02343 goto AKM_restore; 02344 02345 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) 02346 goto AKM_restore; 02347 02348 result = 0; 02349 data = (short)(tmp[1] << 8) | tmp[0]; 02350 if ((data > 100) || (data < -100)) 02351 result |= 0x01; 02352 data = (short)(tmp[3] << 8) | tmp[2]; 02353 if ((data > 100) || (data < -100)) 02354 result |= 0x02; 02355 data = (short)(tmp[5] << 8) | tmp[4]; 02356 if ((data > -300) || (data < -1000)) 02357 result |= 0x04; 02358 02359 AKM_restore: 02360 tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; 02361 i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); 02362 tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; 02363 i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); 02364 mpu_set_bypass(0); 02365 return result; 02366 } 02367 #endif 02368 #endif 02369 02370 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 02371 { 02372 unsigned char data[MAX_PACKET_LENGTH]; 02373 unsigned char packet_count, ii; 02374 unsigned short fifo_count; 02375 02376 data[0] = 0x01; 02377 data[1] = 0; 02378 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 02379 return -1; 02380 delay_ms(200); 02381 data[0] = 0; 02382 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 02383 return -1; 02384 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02385 return -1; 02386 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 02387 return -1; 02388 if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02389 return -1; 02390 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02391 return -1; 02392 data[0] = BIT_FIFO_RST | BIT_DMP_RST; 02393 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02394 return -1; 02395 delay_ms(15); 02396 data[0] = st.test->reg_lpf; 02397 if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) 02398 return -1; 02399 data[0] = st.test->reg_rate_div; 02400 if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) 02401 return -1; 02402 if (hw_test) 02403 data[0] = st.test->reg_gyro_fsr | 0xE0; 02404 else 02405 data[0] = st.test->reg_gyro_fsr; 02406 if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) 02407 return -1; 02408 02409 if (hw_test) 02410 data[0] = st.test->reg_accel_fsr | 0xE0; 02411 else 02412 data[0] = test.reg_accel_fsr; 02413 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 02414 return -1; 02415 if (hw_test) 02416 delay_ms(200); 02417 02418 /* Fill FIFO for test.wait_ms milliseconds. */ 02419 data[0] = BIT_FIFO_EN; 02420 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) 02421 return -1; 02422 02423 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; 02424 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02425 return -1; 02426 delay_ms(test.wait_ms); 02427 data[0] = 0; 02428 if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) 02429 return -1; 02430 02431 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) 02432 return -1; 02433 02434 fifo_count = (data[0] << 8) | data[1]; 02435 packet_count = fifo_count / MAX_PACKET_LENGTH; 02436 gyro[0] = gyro[1] = gyro[2] = 0; 02437 accel[0] = accel[1] = accel[2] = 0; 02438 02439 for (ii = 0; ii < packet_count; ii++) { 02440 short accel_cur[3], gyro_cur[3]; 02441 if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) 02442 return -1; 02443 accel_cur[0] = ((short)data[0] << 8) | data[1]; 02444 accel_cur[1] = ((short)data[2] << 8) | data[3]; 02445 accel_cur[2] = ((short)data[4] << 8) | data[5]; 02446 accel[0] += (long)accel_cur[0]; 02447 accel[1] += (long)accel_cur[1]; 02448 accel[2] += (long)accel_cur[2]; 02449 gyro_cur[0] = (((short)data[6] << 8) | data[7]); 02450 gyro_cur[1] = (((short)data[8] << 8) | data[9]); 02451 gyro_cur[2] = (((short)data[10] << 8) | data[11]); 02452 gyro[0] += (long)gyro_cur[0]; 02453 gyro[1] += (long)gyro_cur[1]; 02454 gyro[2] += (long)gyro_cur[2]; 02455 } 02456 #ifdef EMPL_NO_64BIT 02457 gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); 02458 gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); 02459 gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); 02460 if (has_accel) { 02461 accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / 02462 packet_count); 02463 accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / 02464 packet_count); 02465 accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / 02466 packet_count); 02467 /* Don't remove gravity! */ 02468 accel[2] -= 65536L; 02469 } 02470 #else 02471 gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); 02472 gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); 02473 gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); 02474 accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / 02475 packet_count); 02476 accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / 02477 packet_count); 02478 accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / 02479 packet_count); 02480 /* Don't remove gravity! */ 02481 if (accel[2] > 0L) 02482 accel[2] -= 65536L; 02483 else 02484 accel[2] += 65536L; 02485 #endif 02486 02487 return 0; 02488 } 02489 02490 /** 02491 * @brief Trigger gyro/accel/compass self-test. 02492 * On success/error, the self-test returns a mask representing the sensor(s) 02493 * that failed. For each bit, a one (1) represents a "pass" case; conversely, 02494 * a zero (0) indicates a failure. 02495 * 02496 * \n The mask is defined as follows: 02497 * \n Bit 0: Gyro. 02498 * \n Bit 1: Accel. 02499 * \n Bit 2: Compass. 02500 * 02501 * \n Currently, the hardware self-test is unsupported for MPU6500. However, 02502 * this function can still be used to obtain the accel and gyro biases. 02503 * 02504 * \n This function must be called with the device either face-up or face-down 02505 * (z-axis is parallel to gravity). 02506 * @param[out] gyro Gyro biases in q16 format. 02507 * @param[out] accel Accel biases (if applicable) in q16 format. 02508 * @return Result mask (see above). 02509 */ 02510 int mpu_run_self_test(long *gyro, long *accel) 02511 { 02512 #ifdef MPU6050 02513 const unsigned char tries = 2; 02514 long gyro_st[3], accel_st[3]; 02515 unsigned char accel_result, gyro_result; 02516 #ifdef AK89xx_SECONDARY 02517 unsigned char compass_result; 02518 #endif 02519 int ii; 02520 #endif 02521 int result; 02522 unsigned char accel_fsr, fifo_sensors, sensors_on; 02523 unsigned short gyro_fsr, sample_rate, lpf; 02524 unsigned char dmp_was_on; 02525 02526 if (st.chip_cfg.dmp_on) { 02527 mpu_set_dmp_state(0); 02528 dmp_was_on = 1; 02529 } else 02530 dmp_was_on = 0; 02531 02532 /* Get initial settings. */ 02533 mpu_get_gyro_fsr(&gyro_fsr); 02534 mpu_get_accel_fsr(&accel_fsr); 02535 mpu_get_lpf(&lpf); 02536 mpu_get_sample_rate(&sample_rate); 02537 sensors_on = st.chip_cfg.sensors; 02538 mpu_get_fifo_config(&fifo_sensors); 02539 02540 /* For older chips, the self-test will be different. */ 02541 #if defined MPU6050 02542 for (ii = 0; ii < tries; ii++) 02543 if (!get_st_biases(gyro, accel, 0)) 02544 break; 02545 if (ii == tries) { 02546 /* If we reach this point, we most likely encountered an I2C error. 02547 * We'll just report an error for all three sensors. 02548 */ 02549 result = 0; 02550 goto restore; 02551 } 02552 for (ii = 0; ii < tries; ii++) 02553 if (!get_st_biases(gyro_st, accel_st, 1)) 02554 break; 02555 if (ii == tries) { 02556 /* Again, probably an I2C error. */ 02557 result = 0; 02558 goto restore; 02559 } 02560 accel_result = accel_self_test(accel, accel_st); 02561 gyro_result = gyro_self_test(gyro, gyro_st); 02562 02563 result = 0; 02564 if (!gyro_result) 02565 result |= 0x01; 02566 if (!accel_result) 02567 result |= 0x02; 02568 02569 #ifdef AK89xx_SECONDARY 02570 compass_result = compass_self_test(); 02571 if (!compass_result) 02572 result |= 0x04; 02573 #endif 02574 restore: 02575 #elif defined MPU6500 02576 /* For now, this function will return a "pass" result for all three sensors 02577 * for compatibility with current test applications. 02578 */ 02579 get_st_biases(gyro, accel, 0); 02580 result = 0x7; 02581 #endif 02582 /* Set to invalid values to ensure no I2C writes are skipped. */ 02583 st.chip_cfg.gyro_fsr = 0xFF; 02584 st.chip_cfg.accel_fsr = 0xFF; 02585 st.chip_cfg.lpf = 0xFF; 02586 st.chip_cfg.sample_rate = 0xFFFF; 02587 st.chip_cfg.sensors = 0xFF; 02588 st.chip_cfg.fifo_enable = 0xFF; 02589 st.chip_cfg.clk_src = INV_CLK_PLL; 02590 mpu_set_gyro_fsr(gyro_fsr); 02591 mpu_set_accel_fsr(accel_fsr); 02592 mpu_set_lpf(lpf); 02593 mpu_set_sample_rate(sample_rate); 02594 mpu_set_sensors(sensors_on); 02595 mpu_configure_fifo(fifo_sensors); 02596 02597 if (dmp_was_on) 02598 mpu_set_dmp_state(1); 02599 02600 return result; 02601 } 02602 02603 /** 02604 * @brief Write to the DMP memory. 02605 * This function prevents I2C writes past the bank boundaries. The DMP memory 02606 * is only accessible when the chip is awake. 02607 * @param[in] mem_addr Memory location (bank << 8 | start address) 02608 * @param[in] length Number of bytes to write. 02609 * @param[in] data Bytes to write to memory. 02610 * @return 0 if successful. 02611 */ 02612 int mpu_write_mem(unsigned short mem_addr, unsigned short length, 02613 unsigned char *data) 02614 { 02615 unsigned char tmp[2]; 02616 02617 if (!data) 02618 return -1; 02619 if (!st.chip_cfg.sensors) 02620 return -1; 02621 02622 tmp[0] = (unsigned char)(mem_addr >> 8); 02623 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02624 02625 /* Check bank boundaries. */ 02626 if (tmp[1] + length > st.hw->bank_size) 02627 return -1; 02628 02629 if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02630 return -1; 02631 if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data)) 02632 return -1; 02633 return 0; 02634 } 02635 02636 /** 02637 * @brief Read from the DMP memory. 02638 * This function prevents I2C reads past the bank boundaries. The DMP memory 02639 * is only accessible when the chip is awake. 02640 * @param[in] mem_addr Memory location (bank << 8 | start address) 02641 * @param[in] length Number of bytes to read. 02642 * @param[out] data Bytes read from memory. 02643 * @return 0 if successful. 02644 */ 02645 int mpu_read_mem(unsigned short mem_addr, unsigned short length, 02646 unsigned char *data) 02647 { 02648 unsigned char tmp[2]; 02649 02650 if (!data) 02651 return -1; 02652 if (!st.chip_cfg.sensors) 02653 return -1; 02654 02655 tmp[0] = (unsigned char)(mem_addr >> 8); 02656 tmp[1] = (unsigned char)(mem_addr & 0xFF); 02657 02658 /* Check bank boundaries. */ 02659 if (tmp[1] + length > st.hw->bank_size) 02660 return -1; 02661 02662 if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) 02663 return -1; 02664 if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data)) 02665 return -1; 02666 return 0; 02667 } 02668 02669 /** 02670 * @brief Load and verify DMP image. 02671 * @param[in] length Length of DMP image. 02672 * @param[in] firmware DMP code. 02673 * @param[in] start_addr Starting address of DMP code memory. 02674 * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. 02675 * @return 0 if successful. 02676 */ 02677 int mpu_load_firmware(unsigned short length, const unsigned char *firmware, 02678 unsigned short start_addr, unsigned short sample_rate) 02679 { 02680 unsigned short ii; 02681 unsigned short this_write; 02682 /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ 02683 #define LOAD_CHUNK (16) 02684 unsigned char cur[LOAD_CHUNK], tmp[2]; 02685 02686 if (st.chip_cfg.dmp_loaded) 02687 /* DMP should only be loaded once. */ 02688 return -1; 02689 02690 if (!firmware) 02691 return -1; 02692 for (ii = 0; ii < length; ii += this_write) { 02693 this_write = min(LOAD_CHUNK, length - ii); 02694 if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) 02695 return -1; 02696 if (mpu_read_mem(ii, this_write, cur)) 02697 return -1; 02698 if (memcmp(firmware+ii, cur, this_write)) 02699 return -2; 02700 } 02701 02702 /* Set program start address. */ 02703 tmp[0] = start_addr >> 8; 02704 tmp[1] = start_addr & 0xFF; 02705 if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) 02706 return -1; 02707 02708 st.chip_cfg.dmp_loaded = 1; 02709 st.chip_cfg.dmp_sample_rate = sample_rate; 02710 return 0; 02711 } 02712 02713 /** 02714 * @brief Enable/disable DMP support. 02715 * @param[in] enable 1 to turn on the DMP. 02716 * @return 0 if successful. 02717 */ 02718 int mpu_set_dmp_state(unsigned char enable) 02719 { 02720 unsigned char tmp; 02721 if (st.chip_cfg.dmp_on == enable) 02722 return 0; 02723 02724 if (enable) { 02725 if (!st.chip_cfg.dmp_loaded) 02726 return -1; 02727 /* Disable data ready interrupt. */ 02728 set_int_enable(0); 02729 /* Disable bypass mode. */ 02730 mpu_set_bypass(0); 02731 /* Keep constant sample rate, FIFO rate controlled by DMP. */ 02732 mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); 02733 /* Remove FIFO elements. */ 02734 tmp = 0; 02735 i2c_write(st.hw->addr, 0x23, 1, &tmp); 02736 st.chip_cfg.dmp_on = 1; 02737 /* Enable DMP interrupt. */ 02738 set_int_enable(1); 02739 mpu_reset_fifo(); 02740 } else { 02741 /* Disable DMP interrupt. */ 02742 set_int_enable(0); 02743 /* Restore FIFO settings. */ 02744 tmp = st.chip_cfg.fifo_enable; 02745 i2c_write(st.hw->addr, 0x23, 1, &tmp); 02746 st.chip_cfg.dmp_on = 0; 02747 mpu_reset_fifo(); 02748 } 02749 return 0; 02750 } 02751 02752 /** 02753 * @brief Get DMP state. 02754 * @param[out] enabled 1 if enabled. 02755 * @return 0 if successful. 02756 */ 02757 int mpu_get_dmp_state(unsigned char *enabled) 02758 { 02759 enabled[0] = st.chip_cfg.dmp_on; 02760 return 0; 02761 } 02762 02763 02764 /* This initialization is similar to the one in ak8975.c. */ 02765 int setup_compass(void) 02766 { 02767 #ifdef AK89xx_SECONDARY 02768 unsigned char data[4], akm_addr; 02769 02770 mpu_set_bypass(1); 02771 02772 /* Find compass. Possible addresses range from 0x0C to 0x0F. */ 02773 for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { 02774 int result; 02775 result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); 02776 if (!result && (data[0] == AKM_WHOAMI)) 02777 break; 02778 } 02779 02780 if (akm_addr > 0x0F) { 02781 /* TODO: Handle this case in all compass-related functions. */ 02782 //("Compass not found.\n"); 02783 return -1; 02784 } 02785 02786 st.chip_cfg.compass_addr = akm_addr; 02787 02788 data[0] = AKM_POWER_DOWN; 02789 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02790 return -1; 02791 delay_ms(1); 02792 02793 data[0] = AKM_FUSE_ROM_ACCESS; 02794 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02795 return -1; 02796 delay_ms(1); 02797 02798 /* Get sensitivity adjustment data from fuse ROM. */ 02799 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) 02800 return -1; 02801 st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; 02802 st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; 02803 st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; 02804 02805 data[0] = AKM_POWER_DOWN; 02806 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) 02807 return -1; 02808 delay_ms(1); 02809 02810 mpu_set_bypass(0); 02811 02812 /* Set up master mode, master clock, and ES bit. */ 02813 data[0] = 0x40; 02814 if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) 02815 return -1; 02816 02817 /* Slave 0 reads from AKM data registers. */ 02818 data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; 02819 if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) 02820 return -1; 02821 02822 /* Compass reads start at this register. */ 02823 data[0] = AKM_REG_ST1; 02824 if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) 02825 return -1; 02826 02827 /* Enable slave 0, 8-byte reads. */ 02828 data[0] = BIT_SLAVE_EN | 8; 02829 if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) 02830 return -1; 02831 02832 /* Slave 1 changes AKM measurement mode. */ 02833 data[0] = st.chip_cfg.compass_addr; 02834 if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) 02835 return -1; 02836 02837 /* AKM measurement mode register. */ 02838 data[0] = AKM_REG_CNTL; 02839 if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) 02840 return -1; 02841 02842 /* Enable slave 1, 1-byte writes. */ 02843 data[0] = BIT_SLAVE_EN | 1; 02844 if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) 02845 return -1; 02846 02847 /* Set slave 1 data. */ 02848 data[0] = AKM_SINGLE_MEASUREMENT; 02849 if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) 02850 return -1; 02851 02852 /* Trigger slave 0 and slave 1 actions at each sample. */ 02853 data[0] = 0x03; 02854 if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) 02855 return -1; 02856 02857 #ifdef MPU9150 02858 /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ 02859 data[0] = BIT_I2C_MST_VDDIO; 02860 if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) 02861 return -1; 02862 #endif 02863 02864 return 0; 02865 #else 02866 return -1; 02867 #endif 02868 } 02869 02870 /** 02871 * @brief Read raw compass data. 02872 * @param[out] data Raw data in hardware units. 02873 * @param[out] timestamp Timestamp in milliseconds. Null if not needed. 02874 * @return 0 if successful. 02875 */ 02876 int mpu_get_compass_reg(short *data, unsigned long *timestamp) 02877 { 02878 #ifdef AK89xx_SECONDARY 02879 unsigned char tmp[9]; 02880 02881 if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) 02882 return -1; 02883 02884 #ifdef AK89xx_BYPASS 02885 if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) 02886 return -1; 02887 tmp[8] = AKM_SINGLE_MEASUREMENT; 02888 if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) 02889 return -1; 02890 #else 02891 if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) 02892 return -1; 02893 #endif 02894 02895 #if defined AK8975_SECONDARY 02896 /* AK8975 doesn't have the overrun error bit. */ 02897 if (!(tmp[0] & AKM_DATA_READY)) 02898 return -2; 02899 if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) 02900 return -3; 02901 #elif defined AK8963_SECONDARY 02902 /* AK8963 doesn't have the data read error bit. */ 02903 if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) 02904 return -2; 02905 if (tmp[7] & AKM_OVERFLOW) 02906 return -3; 02907 #endif 02908 data[0] = (tmp[2] << 8) | tmp[1]; 02909 data[1] = (tmp[4] << 8) | tmp[3]; 02910 data[2] = (tmp[6] << 8) | tmp[5]; 02911 02912 data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; 02913 data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; 02914 data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; 02915 02916 if (timestamp) 02917 get_ms(timestamp); 02918 return 0; 02919 #else 02920 return -1; 02921 #endif 02922 } 02923 02924 /** 02925 * @brief Get the compass full-scale range. 02926 * @param[out] fsr Current full-scale range. 02927 * @return 0 if successful. 02928 */ 02929 int mpu_get_compass_fsr(unsigned short *fsr) 02930 { 02931 #ifdef AK89xx_SECONDARY 02932 fsr[0] = st.hw->compass_fsr; 02933 return 0; 02934 #else 02935 return -1; 02936 #endif 02937 } 02938 02939 /** 02940 * @brief Enters LP accel motion interrupt mode. 02941 * The behavior of this feature is very different between the MPU6050 and the 02942 * MPU6500. Each chip's version of this feature is explained below. 02943 * 02944 * \n MPU6050: 02945 * \n When this mode is first enabled, the hardware captures a single accel 02946 * sample, and subsequent samples are compared with this one to determine if 02947 * the device is in motion. Therefore, whenever this "locked" sample needs to 02948 * be changed, this function must be called again. 02949 * 02950 * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg 02951 * increments. 02952 * 02953 * \n Low-power accel mode supports the following frequencies: 02954 * \n 1.25Hz, 5Hz, 20Hz, 40Hz 02955 * 02956 * \n MPU6500: 02957 * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference 02958 * sample. The hardware monitors the accel data and detects any large change 02959 * over a short period of time. 02960 * 02961 * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg 02962 * increments. 02963 * 02964 * \n MPU6500 Low-power accel mode supports the following frequencies: 02965 * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz 02966 * 02967 * \n\n NOTES: 02968 * \n The driver will round down @e thresh to the nearest supported value if 02969 * an unsupported threshold is selected. 02970 * \n To select a fractional wake-up frequency, round down the value passed to 02971 * @e lpa_freq. 02972 * \n The MPU6500 does not support a delay parameter. If this function is used 02973 * for the MPU6500, the value passed to @e time will be ignored. 02974 * \n To disable this mode, set @e lpa_freq to zero. The driver will restore 02975 * the previous configuration. 02976 * 02977 * @param[in] thresh Motion threshold in mg. 02978 * @param[in] time Duration in milliseconds that the accel data must 02979 * exceed @e thresh before motion is reported. 02980 * @param[in] lpa_freq Minimum sampling rate, or zero to disable. 02981 * @return 0 if successful. 02982 */ 02983 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, 02984 unsigned char lpa_freq) 02985 { 02986 unsigned char data[3]; 02987 02988 if (lpa_freq) { 02989 unsigned char thresh_hw; 02990 02991 #if defined MPU6050 02992 /* TODO: Make these const/#defines. */ 02993 /* 1LSb = 32mg. */ 02994 if (thresh > 8160) 02995 thresh_hw = 255; 02996 else if (thresh < 32) 02997 thresh_hw = 1; 02998 else 02999 thresh_hw = thresh >> 5; 03000 #elif defined MPU6500 03001 /* 1LSb = 4mg. */ 03002 if (thresh > 1020) 03003 thresh_hw = 255; 03004 else if (thresh < 4) 03005 thresh_hw = 1; 03006 else 03007 thresh_hw = thresh >> 2; 03008 #endif 03009 03010 if (!time) 03011 /* Minimum duration must be 1ms. */ 03012 time = 1; 03013 03014 #if defined MPU6050 03015 if (lpa_freq > 40) 03016 #elif defined MPU6500 03017 if (lpa_freq > 640) 03018 #endif 03019 /* At this point, the chip has not been re-configured, so the 03020 * function can safely exit. 03021 */ 03022 return -1; 03023 03024 if (!st.chip_cfg.int_motion_only) { 03025 /* Store current settings for later. */ 03026 if (st.chip_cfg.dmp_on) { 03027 mpu_set_dmp_state(0); 03028 st.chip_cfg.cache.dmp_on = 1; 03029 } else 03030 st.chip_cfg.cache.dmp_on = 0; 03031 mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); 03032 mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); 03033 mpu_get_lpf(&st.chip_cfg.cache.lpf); 03034 mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); 03035 st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; 03036 mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); 03037 } 03038 03039 #ifdef MPU6050 03040 /* Disable hardware interrupts for now. */ 03041 set_int_enable(0); 03042 03043 /* Enter full-power accel-only mode. */ 03044 mpu_lp_accel_mode(0); 03045 03046 /* Override current LPF (and HPF) settings to obtain a valid accel 03047 * reading. 03048 */ 03049 data[0] = INV_FILTER_256HZ_NOLPF2; 03050 if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) 03051 return -1; 03052 03053 /* NOTE: Digital high pass filter should be configured here. Since this 03054 * driver doesn't modify those bits anywhere, they should already be 03055 * cleared by default. 03056 */ 03057 03058 /* Configure the device to send motion interrupts. */ 03059 /* Enable motion interrupt. */ 03060 data[0] = BIT_MOT_INT_EN; 03061 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 03062 goto lp_int_restore; 03063 03064 /* Set motion interrupt parameters. */ 03065 data[0] = thresh_hw; 03066 data[1] = time; 03067 if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data)) 03068 goto lp_int_restore; 03069 03070 /* Force hardware to "lock" current accel sample. */ 03071 delay_ms(5); 03072 data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF; 03073 if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) 03074 goto lp_int_restore; 03075 03076 /* Set up LP accel mode. */ 03077 data[0] = BIT_LPA_CYCLE; 03078 if (lpa_freq == 1) 03079 data[1] = INV_LPA_1_25HZ; 03080 else if (lpa_freq <= 5) 03081 data[1] = INV_LPA_5HZ; 03082 else if (lpa_freq <= 20) 03083 data[1] = INV_LPA_20HZ; 03084 else 03085 data[1] = INV_LPA_40HZ; 03086 data[1] = (data[1] << 6) | BIT_STBY_XYZG; 03087 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) 03088 goto lp_int_restore; 03089 03090 st.chip_cfg.int_motion_only = 1; 03091 return 0; 03092 #elif defined MPU6500 03093 /* Disable hardware interrupts. */ 03094 set_int_enable(0); 03095 03096 /* Enter full-power accel-only mode, no FIFO/DMP. */ 03097 data[0] = 0; 03098 data[1] = 0; 03099 data[2] = BIT_STBY_XYZG; 03100 if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data)) 03101 goto lp_int_restore; 03102 03103 /* Set motion threshold. */ 03104 data[0] = thresh_hw; 03105 if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data)) 03106 goto lp_int_restore; 03107 03108 /* Set wake frequency. */ 03109 if (lpa_freq == 1) 03110 data[0] = INV_LPA_1_25HZ; 03111 else if (lpa_freq == 2) 03112 data[0] = INV_LPA_2_5HZ; 03113 else if (lpa_freq <= 5) 03114 data[0] = INV_LPA_5HZ; 03115 else if (lpa_freq <= 10) 03116 data[0] = INV_LPA_10HZ; 03117 else if (lpa_freq <= 20) 03118 data[0] = INV_LPA_20HZ; 03119 else if (lpa_freq <= 40) 03120 data[0] = INV_LPA_40HZ; 03121 else if (lpa_freq <= 80) 03122 data[0] = INV_LPA_80HZ; 03123 else if (lpa_freq <= 160) 03124 data[0] = INV_LPA_160HZ; 03125 else if (lpa_freq <= 320) 03126 data[0] = INV_LPA_320HZ; 03127 else 03128 data[0] = INV_LPA_640HZ; 03129 if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) 03130 goto lp_int_restore; 03131 03132 /* Enable motion interrupt (MPU6500 version). */ 03133 data[0] = BITS_WOM_EN; 03134 if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) 03135 goto lp_int_restore; 03136 03137 /* Enable cycle mode. */ 03138 data[0] = BIT_LPA_CYCLE; 03139 if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) 03140 goto lp_int_restore; 03141 03142 /* Enable interrupt. */ 03143 data[0] = BIT_MOT_INT_EN; 03144 if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) 03145 goto lp_int_restore; 03146 03147 st.chip_cfg.int_motion_only = 1; 03148 return 0; 03149 #endif 03150 } else { 03151 /* Don't "restore" the previous state if no state has been saved. */ 03152 int ii; 03153 char *cache_ptr = (char*)&st.chip_cfg.cache; 03154 for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { 03155 if (cache_ptr[ii] != 0) 03156 goto lp_int_restore; 03157 } 03158 /* If we reach this point, motion interrupt mode hasn't been used yet. */ 03159 return -1; 03160 } 03161 lp_int_restore: 03162 /* Set to invalid values to ensure no I2C writes are skipped. */ 03163 st.chip_cfg.gyro_fsr = 0xFF; 03164 st.chip_cfg.accel_fsr = 0xFF; 03165 st.chip_cfg.lpf = 0xFF; 03166 st.chip_cfg.sample_rate = 0xFFFF; 03167 st.chip_cfg.sensors = 0xFF; 03168 st.chip_cfg.fifo_enable = 0xFF; 03169 st.chip_cfg.clk_src = INV_CLK_PLL; 03170 mpu_set_sensors(st.chip_cfg.cache.sensors_on); 03171 mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); 03172 mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); 03173 mpu_set_lpf(st.chip_cfg.cache.lpf); 03174 mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); 03175 mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); 03176 03177 if (st.chip_cfg.cache.dmp_on) 03178 mpu_set_dmp_state(1); 03179 03180 #ifdef MPU6500 03181 /* Disable motion interrupt (MPU6500 version). */ 03182 data[0] = 0; 03183 if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) 03184 goto lp_int_restore; 03185 #endif 03186 03187 st.chip_cfg.int_motion_only = 0; 03188 return 0; 03189 } 03190 ////////////////////////////////////////////////////////////////////////////////// 03191 //添加的代码部分 03192 ////////////////////////////////////////////////////////////////////////////////// 03193 //本程序只供学习使用，未经作者许可，不得用于其它任何用途 03194 //ALIENTEK STM32开发板 03195 //MPU6050 DMP 驱动代码 03196 //正点原子@ALIENTEK 03197 //技术论坛:www.openedv.com 03198 //创建日期:2015/1/17 03199 //版本：V1.0 03200 //版权所有，盗版必究。 03201 //Copyright(C) 广州市星翼电子科技有限公司 2009-2019 03202 //All rights reserved 03203 ////////////////////////////////////////////////////////////////////////////////// 03204 03205 //q30格式,long转float时的除数. 03206 #define q30 1073741824.0f 03207 03208 //陀螺仪方向设置 03209 static signed char gyro_orientation[9] = { 1, 0, 0, 03210 0, 1, 0, 03211 0, 0, 1}; 03212 //MPU6050自测试 03213 //返回值:0,正常 03214 // 其他,失败 03215 unsigned char run_self_test(void) 03216 { 03217 int result; 03218 //char test_packet[4] = {0}; 03219 long gyro[3], accel[3]; 03220 result = mpu_run_self_test(gyro, accel); 03221 if (result == 0x3) 03222 { 03223 /* Test passed. We can trust the gyro data here, so let's push it down 03224 * to the DMP. 03225 */ 03226 float sens; 03227 unsigned short accel_sens; 03228 mpu_get_gyro_sens(&sens); 03229 gyro[0] = (long)(gyro[0] * sens); 03230 gyro[1] = (long)(gyro[1] * sens); 03231 gyro[2] = (long)(gyro[2] * sens); 03232 dmp_set_gyro_bias(gyro); 03233 mpu_get_accel_sens(&accel_sens); 03234 accel[0] *= accel_sens; 03235 accel[1] *= accel_sens; 03236 accel[2] *= accel_sens; 03237 dmp_set_accel_bias(accel); 03238 return 0; 03239 }else return 1; 03240 } 03241 //陀螺仪方向控制 03242 unsigned short inv_orientation_matrix_to_scalar( 03243 const signed char *mtx) 03244 { 03245 unsigned short scalar; 03246 /* 03247 XYZ 010_001_000 Identity Matrix 03248 XZY 001_010_000 03249 YXZ 010_000_001 03250 YZX 000_010_001 03251 ZXY 001_000_010 03252 ZYX 000_001_010 03253 */ 03254 03255 scalar = inv_row_2_scale(mtx); 03256 scalar |= inv_row_2_scale(mtx + 3) << 3; 03257 scalar |= inv_row_2_scale(mtx + 6) << 6; 03258 03259 03260 return scalar; 03261 } 03262 //方向转换 03263 unsigned short inv_row_2_scale(const signed char *row) 03264 { 03265 unsigned short b; 03266 03267 if (row[0] > 0) 03268 b = 0; 03269 else if (row[0] < 0) 03270 b = 4; 03271 else if (row[1] > 0) 03272 b = 1; 03273 else if (row[1] < 0) 03274 b = 5; 03275 else if (row[2] > 0) 03276 b = 2; 03277 else if (row[2] < 0) 03278 b = 6; 03279 else 03280 b = 7; // error 03281 return b; 03282 } 03283 //空函数,未用到. 03284 void mget_ms(unsigned long *time) 03285 { 03286 03287 } 03288 //mpu6050,dmp初始化 03289 //返回值:0,正常 03290 // 其他,失败 03291 unsigned char mpu_dmp_init(void) 03292 { 03293 unsigned char res=0; 03294 MPU_IIC_Init(); //初始化IIC总线 03295 if(mpu_init()==0) //初始化MPU6050 03296 { 03297 res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器 03298 if(res)return 1; 03299 res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO 03300 if(res)return 2; 03301 res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 03302 if(res)return 3; 03303 res=dmp_load_motion_driver_firmware(); //加载dmp固件 03304 if(res)return 4; 03305 res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向 03306 if(res)return 5; 03307 res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能 03308 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| 03309 DMP_FEATURE_GYRO_CAL); 03310 if(res)return 6; 03311 res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz) 03312 if(res)return 7; 03313 res=run_self_test(); //自检 03314 if(res)return 8; 03315 res=mpu_set_dmp_state(1); //使能DMP 03316 if(res)return 9; 03317 }else return 10; 03318 return 0; 03319 } 03320 //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多) 03321 //pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0° 03322 //roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0° 03323 //yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0° 03324 //返回值:0,正常 03325 // 其他,失败 03326 unsigned char mpu_dmp_get_data(float *pitch,float *roll,float *yaw) 03327 { 03328 float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; 03329 unsigned long sensor_timestamp; 03330 short gyro[3], accel[3], sensors; 03331 unsigned char more; 03332 long quat[4]; 03333 if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; 03334 /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units. 03335 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent. 03336 **/ 03337 /*if (sensors & INV_XYZ_GYRO ) 03338 send_packet(PACKET_TYPE_GYRO, gyro); 03339 if (sensors & INV_XYZ_ACCEL) 03340 send_packet(PACKET_TYPE_ACCEL, accel); */ 03341 /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. 03342 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 03343 **/ 03344 if(sensors&INV_WXYZ_QUAT) 03345 { 03346 q0 = quat[0] / q30; //q30格式转换为浮点数 03347 q1 = quat[1] / q30; 03348 q2 = quat[2] / q30; 03349 q3 = quat[3] / q30; 03350 //计算得到俯仰角/横滚角/航向角 03351 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 03352 *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll 03353 *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw 03354 }else return 2; 03355 return 0; 03356 } 03357 03358 //定义目标板采用MSP430 03359 #define MOTION_DRIVER_TARGET_MSP430 03360 03361 /* The following functions must be defined for this platform: 03362 * i2c_write(unsigned char slave_addr, unsigned char reg_addr, 03363 * unsigned char length, unsigned char const *data) 03364 * i2c_read(unsigned char slave_addr, unsigned char reg_addr, 03365 * unsigned char length, unsigned char *data) 03366 * delay_ms(unsigned long num_ms) 03367 * get_ms(unsigned long *count) 03368 */ 03369 #if defined MOTION_DRIVER_TARGET_MSP430 03370 //#include "msp430.h" 03371 //#include "msp430_clock.h" 03372 //#define delay_ms delay_ms 03373 #define get_ms mget_ms 03374 //#define // printf 03375 //#define // printf 03376 03377 #elif defined EMPL_TARGET_MSP430 03378 #include "msp430.h" 03379 #include "msp430_clock.h" 03380 #include "log.h" 03381 #define delay_ms msp430_delay_ms 03382 #define get_ms msp430_get_clock_ms 03383 //#define // MPL_LOGI 03384 //#define // MPL_LOGE 03385 03386 #elif defined EMPL_TARGET_UC3L0 03387 /* Instead of using the standard TWI driver from the ASF library, we're using 03388 * a TWI driver that follows the slave address + register address convention. 03389 */ 03390 #include "delay.h" 03391 #include "sysclk.h" 03392 #include "log.h" 03393 #include "uc3l0_clock.h" 03394 /* delay_ms is a function already defined in ASF. */ 03395 #define get_ms uc3l0_get_clock_ms 03396 //#define // MPL_LOGI 03397 //#define // MPL_LOGE 03398 03399 #else 03400 #error Gyro driver is missing the system layer implementations. 03401 #endif 03402 03403 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL 03404 * releases. These defines may change for each DMP image, so be sure to modify 03405 * these values when switching to a new image. 03406 */ 03407 #define CFG_LP_QUAT (2712) 03408 #define END_ORIENT_TEMP (1866) 03409 #define CFG_27 (2742) 03410 #define CFG_20 (2224) 03411 #define CFG_23 (2745) 03412 #define CFG_FIFO_ON_EVENT (2690) 03413 #define END_PREDICTION_UPDATE (1761) 03414 #define CGNOTICE_INTR (2620) 03415 #define X_GRT_Y_TMP (1358) 03416 #define CFG_DR_INT (1029) 03417 #define CFG_AUTH (1035) 03418 #define UPDATE_PROP_ROT (1835) 03419 #define END_COMPARE_Y_X_TMP2 (1455) 03420 #define SKIP_X_GRT_Y_TMP (1359) 03421 #define SKIP_END_COMPARE (1435) 03422 #define FCFG_3 (1088) 03423 #define FCFG_2 (1066) 03424 #define FCFG_1 (1062) 03425 #define END_COMPARE_Y_X_TMP3 (1434) 03426 #define FCFG_7 (1073) 03427 #define FCFG_6 (1106) 03428 #define FLAT_STATE_END (1713) 03429 #define SWING_END_4 (1616) 03430 #define SWING_END_2 (1565) 03431 #define SWING_END_3 (1587) 03432 #define SWING_END_1 (1550) 03433 #define CFG_8 (2718) 03434 #define CFG_15 (2727) 03435 #define CFG_16 (2746) 03436 #define CFG_EXT_GYRO_BIAS (1189) 03437 #define END_COMPARE_Y_X_TMP (1407) 03438 #define DO_NOT_UPDATE_PROP_ROT (1839) 03439 #define CFG_7 (1205) 03440 #define FLAT_STATE_END_TEMP (1683) 03441 #define END_COMPARE_Y_X (1484) 03442 #define SKIP_SWING_END_1 (1551) 03443 #define SKIP_SWING_END_3 (1588) 03444 #define SKIP_SWING_END_2 (1566) 03445 #define TILTG75_START (1672) 03446 #define CFG_6 (2753) 03447 #define TILTL75_END (1669) 03448 #define END_ORIENT (1884) 03449 #define CFG_FLICK_IN (2573) 03450 #define TILTL75_START (1643) 03451 #define CFG_MOTION_BIAS (1208) 03452 #define X_GRT_Y (1408) 03453 #define TEMPLABEL (2324) 03454 #define CFG_ANDROID_ORIENT_INT (1853) 03455 #define CFG_GYRO_RAW_DATA (2722) 03456 #define X_GRT_Y_TMP2 (1379) 03457 03458 #define D_0_22 (22+512) 03459 #define D_0_24 (24+512) 03460 03461 #define D_0_36 (36) 03462 #define D_0_52 (52) 03463 #define D_0_96 (96) 03464 #define D_0_104 (104) 03465 #define D_0_108 (108) 03466 #define D_0_163 (163) 03467 #define D_0_188 (188) 03468 #define D_0_192 (192) 03469 #define D_0_224 (224) 03470 #define D_0_228 (228) 03471 #define D_0_232 (232) 03472 #define D_0_236 (236) 03473 03474 #define D_1_2 (256 + 2) 03475 #define D_1_4 (256 + 4) 03476 #define D_1_8 (256 + 8) 03477 #define D_1_10 (256 + 10) 03478 #define D_1_24 (256 + 24) 03479 #define D_1_28 (256 + 28) 03480 #define D_1_36 (256 + 36) 03481 #define D_1_40 (256 + 40) 03482 #define D_1_44 (256 + 44) 03483 #define D_1_72 (256 + 72) 03484 #define D_1_74 (256 + 74) 03485 #define D_1_79 (256 + 79) 03486 #define D_1_88 (256 + 88) 03487 #define D_1_90 (256 + 90) 03488 #define D_1_92 (256 + 92) 03489 #define D_1_96 (256 + 96) 03490 #define D_1_98 (256 + 98) 03491 #define D_1_106 (256 + 106) 03492 #define D_1_108 (256 + 108) 03493 #define D_1_112 (256 + 112) 03494 #define D_1_128 (256 + 144) 03495 #define D_1_152 (256 + 12) 03496 #define D_1_160 (256 + 160) 03497 #define D_1_176 (256 + 176) 03498 #define D_1_178 (256 + 178) 03499 #define D_1_218 (256 + 218) 03500 #define D_1_232 (256 + 232) 03501 #define D_1_236 (256 + 236) 03502 #define D_1_240 (256 + 240) 03503 #define D_1_244 (256 + 244) 03504 #define D_1_250 (256 + 250) 03505 #define D_1_252 (256 + 252) 03506 #define D_2_12 (512 + 12) 03507 #define D_2_96 (512 + 96) 03508 #define D_2_108 (512 + 108) 03509 #define D_2_208 (512 + 208) 03510 #define D_2_224 (512 + 224) 03511 #define D_2_236 (512 + 236) 03512 #define D_2_244 (512 + 244) 03513 #define D_2_248 (512 + 248) 03514 #define D_2_252 (512 + 252) 03515 03516 #define CPASS_BIAS_X (35 * 16 + 4) 03517 #define CPASS_BIAS_Y (35 * 16 + 8) 03518 #define CPASS_BIAS_Z (35 * 16 + 12) 03519 #define CPASS_MTX_00 (36 * 16) 03520 #define CPASS_MTX_01 (36 * 16 + 4) 03521 #define CPASS_MTX_02 (36 * 16 + 8) 03522 #define CPASS_MTX_10 (36 * 16 + 12) 03523 #define CPASS_MTX_11 (37 * 16) 03524 #define CPASS_MTX_12 (37 * 16 + 4) 03525 #define CPASS_MTX_20 (37 * 16 + 8) 03526 #define CPASS_MTX_21 (37 * 16 + 12) 03527 #define CPASS_MTX_22 (43 * 16 + 12) 03528 #define D_EXT_GYRO_BIAS_X (61 * 16) 03529 #define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 03530 #define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 03531 #define D_ACT0 (40 * 16) 03532 #define D_ACSX (40 * 16 + 4) 03533 #define D_ACSY (40 * 16 + 8) 03534 #define D_ACSZ (40 * 16 + 12) 03535 03536 #define FLICK_MSG (45 * 16 + 4) 03537 #define FLICK_COUNTER (45 * 16 + 8) 03538 #define FLICK_LOWER (45 * 16 + 12) 03539 #define FLICK_UPPER (46 * 16 + 12) 03540 03541 #define D_AUTH_OUT (992) 03542 #define D_AUTH_IN (996) 03543 #define D_AUTH_A (1000) 03544 #define D_AUTH_B (1004) 03545 03546 #define D_PEDSTD_BP_B (768 + 0x1C) 03547 #define D_PEDSTD_HP_A (768 + 0x78) 03548 #define D_PEDSTD_HP_B (768 + 0x7C) 03549 #define D_PEDSTD_BP_A4 (768 + 0x40) 03550 #define D_PEDSTD_BP_A3 (768 + 0x44) 03551 #define D_PEDSTD_BP_A2 (768 + 0x48) 03552 #define D_PEDSTD_BP_A1 (768 + 0x4C) 03553 #define D_PEDSTD_INT_THRSH (768 + 0x68) 03554 #define D_PEDSTD_CLIP (768 + 0x6C) 03555 #define D_PEDSTD_SB (768 + 0x28) 03556 #define D_PEDSTD_SB_TIME (768 + 0x2C) 03557 #define D_PEDSTD_PEAKTHRSH (768 + 0x98) 03558 #define D_PEDSTD_TIML (768 + 0x2A) 03559 #define D_PEDSTD_TIMH (768 + 0x2E) 03560 #define D_PEDSTD_PEAK (768 + 0X94) 03561 #define D_PEDSTD_STEPCTR (768 + 0x60) 03562 #define D_PEDSTD_TIMECTR (964) 03563 #define D_PEDSTD_DECI (768 + 0xA0) 03564 03565 #define D_HOST_NO_MOT (976) 03566 #define D_ACCEL_BIAS (660) 03567 03568 #define D_ORIENT_GAP (76) 03569 03570 #define D_TILT0_H (48) 03571 #define D_TILT0_L (50) 03572 #define D_TILT1_H (52) 03573 #define D_TILT1_L (54) 03574 #define D_TILT2_H (56) 03575 #define D_TILT2_L (58) 03576 #define D_TILT3_H (60) 03577 #define D_TILT3_L (62) 03578 03579 #define DMP_CODE_SIZE (3062) 03580 03581 static const unsigned char dmp_memory[DMP_CODE_SIZE] = { 03582 /* bank # 0 */ 03583 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 03584 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 03585 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 03586 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 03587 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 03588 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, 03589 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 03590 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 03591 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 03592 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 03593 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 03594 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 03595 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 03596 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 03597 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, 03598 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03599 /* bank # 1 */ 03600 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 03601 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 03602 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 03603 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03604 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 03605 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 03606 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 03607 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 03608 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03609 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03610 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 03611 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 03612 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 03613 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 03614 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 03615 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 03616 /* bank # 2 */ 03617 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 03618 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, 03619 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 03620 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 03621 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03622 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03623 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03624 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03625 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03626 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03627 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03628 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03629 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 03630 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 03631 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 03632 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03633 /* bank # 3 */ 03634 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03635 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 03636 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 03637 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03638 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 03639 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03640 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 03641 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 03642 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03643 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 03644 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03645 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03646 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03647 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03648 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 03649 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 03650 03651 /* bank # 4 */ 03652 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 03653 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 03654 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, 03655 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, 03656 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 03657 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 03658 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 03659 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 03660 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 03661 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, 03662 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 03663 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 03664 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 03665 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, 03666 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 03667 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, 03668 /* bank # 5 */ 03669 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, 03670 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, 03671 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 03672 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 03673 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 03674 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 03675 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 03676 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 03677 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 03678 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 03679 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 03680 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 03681 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 03682 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 03683 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, 03684 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, 03685 /* bank # 6 */ 03686 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, 03687 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, 03688 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, 03689 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 03690 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, 03691 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, 03692 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, 03693 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, 03694 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, 03695 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, 03696 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, 03697 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, 03698 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, 03699 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, 03700 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, 03701 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, 03702 /* bank # 7 */ 03703 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, 03704 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, 03705 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, 03706 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, 03707 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, 03708 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, 03709 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 03710 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 03711 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 03712 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 03713 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 03714 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 03715 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 03716 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 03717 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 03718 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 03719 /* bank # 8 */ 03720 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 03721 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 03722 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 03723 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 03724 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 03725 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 03726 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 03727 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 03728 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 03729 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 03730 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 03731 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 03732 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 03733 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 03734 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 03735 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 03736 /* bank # 9 */ 03737 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 03738 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 03739 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 03740 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 03741 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 03742 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 03743 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 03744 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 03745 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 03746 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 03747 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 03748 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 03749 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, 03750 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, 03751 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, 03752 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 03753 /* bank # 10 */ 03754 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 03755 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 03756 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 03757 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 03758 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 03759 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 03760 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 03761 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, 03762 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, 03763 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 03764 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 03765 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 03766 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 03767 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 03768 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 03769 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 03770 /* bank # 11 */ 03771 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 03772 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 03773 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 03774 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 03775 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 03776 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 03777 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 03778 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 03779 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 03780 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 03781 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 03782 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 03783 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 03784 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 03785 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 03786 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff 03787 }; 03788 03789 static const unsigned short sStartAddress = 0x0400; 03790 03791 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ 03792 03793 #define INT_SRC_TAP (0x01) 03794 #define INT_SRC_ANDROID_ORIENT (0x08) 03795 03796 #define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ 03797 DMP_FEATURE_SEND_CAL_GYRO) 03798 03799 #define MAX_PACKET_LENGTH_2 (32) //前面已经有一个定义了你，避免冲突改成2 03800 03801 #define DMP_SAMPLE_RATE (200) 03802 #define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) 03803 03804 #define FIFO_CORRUPTION_CHECK 03805 #ifdef FIFO_CORRUPTION_CHECK 03806 #define QUAT_ERROR_THRESH (1L<<24) 03807 #define QUAT_MAG_SQ_NORMALIZED (1L<<28) 03808 #define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) 03809 #define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) 03810 #endif 03811 03812 struct dmp_s { 03813 void (*tap_cb)(unsigned char count, unsigned char direction); 03814 void (*android_orient_cb)(unsigned char orientation); 03815 unsigned short orient; 03816 unsigned short feature_mask; 03817 unsigned short fifo_rate; 03818 unsigned char packet_length; 03819 }; 03820 03821 //static struct dmp_s dmp = { 03822 // .tap_cb = NULL, 03823 // .android_orient_cb = NULL, 03824 // .orient = 0, 03825 // .feature_mask = 0, 03826 // .fifo_rate = 0, 03827 // .packet_length = 0 03828 //}; 03829 03830 static struct dmp_s dmp={ 03831 NULL, 03832 NULL, 03833 0, 03834 0, 03835 0, 03836 0 03837 }; 03838 03839 /** 03840 * @brief Load the DMP with this image. 03841 * @return 0 if successful. 03842 */ 03843 int dmp_load_motion_driver_firmware(void) 03844 { 03845 return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, 03846 DMP_SAMPLE_RATE); 03847 } 03848 03849 /** 03850 * @brief Push gyro and accel orientation to the DMP. 03851 * The orientation is represented here as the output of 03852 * @e inv_orientation_matrix_to_scalar. 03853 * @param[in] orient Gyro and accel orientation in body frame. 03854 * @return 0 if successful. 03855 */ 03856 int dmp_set_orientation(unsigned short orient) 03857 { 03858 unsigned char gyro_regs[3], accel_regs[3]; 03859 const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; 03860 const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; 03861 const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; 03862 const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; 03863 03864 gyro_regs[0] = gyro_axes[orient & 3]; 03865 gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; 03866 gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; 03867 accel_regs[0] = accel_axes[orient & 3]; 03868 accel_regs[1] = accel_axes[(orient >> 3) & 3]; 03869 accel_regs[2] = accel_axes[(orient >> 6) & 3]; 03870 03871 /* Chip-to-body, axes only. */ 03872 if (mpu_write_mem(FCFG_1, 3, gyro_regs)) 03873 return -1; 03874 if (mpu_write_mem(FCFG_2, 3, accel_regs)) 03875 return -1; 03876 03877 memcpy(gyro_regs, gyro_sign, 3); 03878 memcpy(accel_regs, accel_sign, 3); 03879 if (orient & 4) { 03880 gyro_regs[0] |= 1; 03881 accel_regs[0] |= 1; 03882 } 03883 if (orient & 0x20) { 03884 gyro_regs[1] |= 1; 03885 accel_regs[1] |= 1; 03886 } 03887 if (orient & 0x100) { 03888 gyro_regs[2] |= 1; 03889 accel_regs[2] |= 1; 03890 } 03891 03892 /* Chip-to-body, sign only. */ 03893 if (mpu_write_mem(FCFG_3, 3, gyro_regs)) 03894 return -1; 03895 if (mpu_write_mem(FCFG_7, 3, accel_regs)) 03896 return -1; 03897 dmp.orient = orient; 03898 return 0; 03899 } 03900 03901 /** 03902 * @brief Push gyro biases to the DMP. 03903 * Because the gyro integration is handled in the DMP, any gyro biases 03904 * calculated by the MPL should be pushed down to DMP memory to remove 03905 * 3-axis quaternion drift. 03906 * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will 03907 * overwrite the biases written to this location once a new one is computed. 03908 * @param[in] bias Gyro biases in q16. 03909 * @return 0 if successful. 03910 */ 03911 int dmp_set_gyro_bias(long *bias) 03912 { 03913 long gyro_bias_body[3]; 03914 unsigned char regs[4]; 03915 03916 gyro_bias_body[0] = bias[dmp.orient & 3]; 03917 if (dmp.orient & 4) 03918 gyro_bias_body[0] *= -1; 03919 gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 03920 if (dmp.orient & 0x20) 03921 gyro_bias_body[1] *= -1; 03922 gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 03923 if (dmp.orient & 0x100) 03924 gyro_bias_body[2] *= -1; 03925 03926 #ifdef EMPL_NO_64BIT 03927 gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); 03928 gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); 03929 gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); 03930 #else 03931 gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); 03932 gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); 03933 gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); 03934 #endif 03935 03936 regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); 03937 regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); 03938 regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); 03939 regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); 03940 if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) 03941 return -1; 03942 03943 regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); 03944 regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); 03945 regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); 03946 regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); 03947 if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) 03948 return -1; 03949 03950 regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); 03951 regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); 03952 regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); 03953 regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); 03954 return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); 03955 } 03956 03957 /** 03958 * @brief Push accel biases to the DMP. 03959 * These biases will be removed from the DMP 6-axis quaternion. 03960 * @param[in] bias Accel biases in q16. 03961 * @return 0 if successful. 03962 */ 03963 int dmp_set_accel_bias(long *bias) 03964 { 03965 long accel_bias_body[3]; 03966 unsigned char regs[12]; 03967 long long accel_sf; 03968 unsigned short accel_sens; 03969 03970 mpu_get_accel_sens(&accel_sens); 03971 accel_sf = (long long)accel_sens << 15; 03972 //__no_operation(); 03973 03974 accel_bias_body[0] = bias[dmp.orient & 3]; 03975 if (dmp.orient & 4) 03976 accel_bias_body[0] *= -1; 03977 accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 03978 if (dmp.orient & 0x20) 03979 accel_bias_body[1] *= -1; 03980 accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 03981 if (dmp.orient & 0x100) 03982 accel_bias_body[2] *= -1; 03983 03984 #ifdef EMPL_NO_64BIT 03985 accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); 03986 accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); 03987 accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); 03988 #else 03989 accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); 03990 accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); 03991 accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); 03992 #endif 03993 03994 regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); 03995 regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); 03996 regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); 03997 regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); 03998 regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); 03999 regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); 04000 regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); 04001 regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); 04002 regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); 04003 regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); 04004 regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); 04005 regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); 04006 return mpu_write_mem(D_ACCEL_BIAS, 12, regs); 04007 } 04008 04009 /** 04010 * @brief Set DMP output rate. 04011 * Only used when DMP is on. 04012 * @param[in] rate Desired fifo rate (Hz). 04013 * @return 0 if successful. 04014 */ 04015 int dmp_set_fifo_rate(unsigned short rate) 04016 { 04017 const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, 04018 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; 04019 unsigned short div; 04020 unsigned char tmp[8]; 04021 04022 if (rate > DMP_SAMPLE_RATE) 04023 return -1; 04024 div = DMP_SAMPLE_RATE / rate - 1; 04025 tmp[0] = (unsigned char)((div >> 8) & 0xFF); 04026 tmp[1] = (unsigned char)(div & 0xFF); 04027 if (mpu_write_mem(D_0_22, 2, tmp)) 04028 return -1; 04029 if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) 04030 return -1; 04031 04032 dmp.fifo_rate = rate; 04033 return 0; 04034 } 04035 04036 /** 04037 * @brief Get DMP output rate. 04038 * @param[out] rate Current fifo rate (Hz). 04039 * @return 0 if successful. 04040 */ 04041 int dmp_get_fifo_rate(unsigned short *rate) 04042 { 04043 rate[0] = dmp.fifo_rate; 04044 return 0; 04045 } 04046 04047 /** 04048 * @brief Set tap threshold for a specific axis. 04049 * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. 04050 * @param[in] thresh Tap threshold, in mg/ms. 04051 * @return 0 if successful. 04052 */ 04053 int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) 04054 { 04055 unsigned char tmp[4], accel_fsr; 04056 float scaled_thresh; 04057 unsigned short dmp_thresh, dmp_thresh_2; 04058 if (!(axis & TAP_XYZ) || thresh > 1600) 04059 return -1; 04060 04061 scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; 04062 04063 mpu_get_accel_fsr(&accel_fsr); 04064 switch (accel_fsr) { 04065 case 2: 04066 dmp_thresh = (unsigned short)(scaled_thresh * 16384); 04067 /* dmp_thresh * 0.75 */ 04068 dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); 04069 break; 04070 case 4: 04071 dmp_thresh = (unsigned short)(scaled_thresh * 8192); 04072 /* dmp_thresh * 0.75 */ 04073 dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); 04074 break; 04075 case 8: 04076 dmp_thresh = (unsigned short)(scaled_thresh * 4096); 04077 /* dmp_thresh * 0.75 */ 04078 dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); 04079 break; 04080 case 16: 04081 dmp_thresh = (unsigned short)(scaled_thresh * 2048); 04082 /* dmp_thresh * 0.75 */ 04083 dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); 04084 break; 04085 default: 04086 return -1; 04087 } 04088 tmp[0] = (unsigned char)(dmp_thresh >> 8); 04089 tmp[1] = (unsigned char)(dmp_thresh & 0xFF); 04090 tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); 04091 tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); 04092 04093 if (axis & TAP_X) { 04094 if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) 04095 return -1; 04096 if (mpu_write_mem(D_1_36, 2, tmp+2)) 04097 return -1; 04098 } 04099 if (axis & TAP_Y) { 04100 if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) 04101 return -1; 04102 if (mpu_write_mem(D_1_40, 2, tmp+2)) 04103 return -1; 04104 } 04105 if (axis & TAP_Z) { 04106 if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) 04107 return -1; 04108 if (mpu_write_mem(D_1_44, 2, tmp+2)) 04109 return -1; 04110 } 04111 return 0; 04112 } 04113 04114 /** 04115 * @brief Set which axes will register a tap. 04116 * @param[in] axis 1, 2, and 4 for XYZ, respectively. 04117 * @return 0 if successful. 04118 */ 04119 int dmp_set_tap_axes(unsigned char axis) 04120 { 04121 unsigned char tmp = 0; 04122 04123 if (axis & TAP_X) 04124 tmp |= 0x30; 04125 if (axis & TAP_Y) 04126 tmp |= 0x0C; 04127 if (axis & TAP_Z) 04128 tmp |= 0x03; 04129 return mpu_write_mem(D_1_72, 1, &tmp); 04130 } 04131 04132 /** 04133 * @brief Set minimum number of taps needed for an interrupt. 04134 * @param[in] min_taps Minimum consecutive taps (1-4). 04135 * @return 0 if successful. 04136 */ 04137 int dmp_set_tap_count(unsigned char min_taps) 04138 { 04139 unsigned char tmp; 04140 04141 if (min_taps < 1) 04142 min_taps = 1; 04143 else if (min_taps > 4) 04144 min_taps = 4; 04145 04146 tmp = min_taps - 1; 04147 return mpu_write_mem(D_1_79, 1, &tmp); 04148 } 04149 04150 /** 04151 * @brief Set length between valid taps. 04152 * @param[in] time Milliseconds between taps. 04153 * @return 0 if successful. 04154 */ 04155 int dmp_set_tap_time(unsigned short time) 04156 { 04157 unsigned short dmp_time; 04158 unsigned char tmp[2]; 04159 04160 dmp_time = time / (1000 / DMP_SAMPLE_RATE); 04161 tmp[0] = (unsigned char)(dmp_time >> 8); 04162 tmp[1] = (unsigned char)(dmp_time & 0xFF); 04163 return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); 04164 } 04165 04166 /** 04167 * @brief Set max time between taps to register as a multi-tap. 04168 * @param[in] time Max milliseconds between taps. 04169 * @return 0 if successful. 04170 */ 04171 int dmp_set_tap_time_multi(unsigned short time) 04172 { 04173 unsigned short dmp_time; 04174 unsigned char tmp[2]; 04175 04176 dmp_time = time / (1000 / DMP_SAMPLE_RATE); 04177 tmp[0] = (unsigned char)(dmp_time >> 8); 04178 tmp[1] = (unsigned char)(dmp_time & 0xFF); 04179 return mpu_write_mem(D_1_218, 2, tmp); 04180 } 04181 04182 /** 04183 * @brief Set shake rejection threshold. 04184 * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. 04185 * @param[in] sf Gyro scale factor. 04186 * @param[in] thresh Gyro threshold in dps. 04187 * @return 0 if successful. 04188 */ 04189 int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) 04190 { 04191 unsigned char tmp[4]; 04192 long thresh_scaled = sf / 1000 * thresh; 04193 tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); 04194 tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); 04195 tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); 04196 tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); 04197 return mpu_write_mem(D_1_92, 4, tmp); 04198 } 04199 04200 /** 04201 * @brief Set shake rejection time. 04202 * Sets the length of time that the gyro must be outside of the threshold set 04203 * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory 04204 * 60 ms is added to this parameter. 04205 * @param[in] time Time in milliseconds. 04206 * @return 0 if successful. 04207 */ 04208 int dmp_set_shake_reject_time(unsigned short time) 04209 { 04210 unsigned char tmp[2]; 04211 04212 time /= (1000 / DMP_SAMPLE_RATE); 04213 tmp[0] = time >> 8; 04214 tmp[1] = time & 0xFF; 04215 return mpu_write_mem(D_1_90,2,tmp); 04216 } 04217 04218 /** 04219 * @brief Set shake rejection timeout. 04220 * Sets the length of time after a shake rejection that the gyro must stay 04221 * inside of the threshold before taps can be detected again. A mandatory 04222 * 60 ms is added to this parameter. 04223 * @param[in] time Time in milliseconds. 04224 * @return 0 if successful. 04225 */ 04226 int dmp_set_shake_reject_timeout(unsigned short time) 04227 { 04228 unsigned char tmp[2]; 04229 04230 time /= (1000 / DMP_SAMPLE_RATE); 04231 tmp[0] = time >> 8; 04232 tmp[1] = time & 0xFF; 04233 return mpu_write_mem(D_1_88,2,tmp); 04234 } 04235 04236 /** 04237 * @brief Get current step count. 04238 * @param[out] count Number of steps detected. 04239 * @return 0 if successful. 04240 */ 04241 int dmp_get_pedometer_step_count(unsigned long *count) 04242 { 04243 unsigned char tmp[4]; 04244 if (!count) 04245 return -1; 04246 04247 if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) 04248 return -1; 04249 04250 count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 04251 ((unsigned long)tmp[2] << 8) | tmp[3]; 04252 return 0; 04253 } 04254 04255 /** 04256 * @brief Overwrite current step count. 04257 * WARNING: This function writes to DMP memory and could potentially encounter 04258 * a race condition if called while the pedometer is enabled. 04259 * @param[in] count New step count. 04260 * @return 0 if successful. 04261 */ 04262 int dmp_set_pedometer_step_count(unsigned long count) 04263 { 04264 unsigned char tmp[4]; 04265 04266 tmp[0] = (unsigned char)((count >> 24) & 0xFF); 04267 tmp[1] = (unsigned char)((count >> 16) & 0xFF); 04268 tmp[2] = (unsigned char)((count >> 8) & 0xFF); 04269 tmp[3] = (unsigned char)(count & 0xFF); 04270 return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); 04271 } 04272 04273 /** 04274 * @brief Get duration of walking time. 04275 * @param[in] time Walk time in milliseconds. 04276 * @return 0 if successful. 04277 */ 04278 int dmp_get_pedometer_walk_time(unsigned long *time) 04279 { 04280 unsigned char tmp[4]; 04281 if (!time) 04282 return -1; 04283 04284 if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) 04285 return -1; 04286 04287 time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 04288 ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; 04289 return 0; 04290 } 04291 04292 /** 04293 * @brief Overwrite current walk time. 04294 * WARNING: This function writes to DMP memory and could potentially encounter 04295 * a race condition if called while the pedometer is enabled. 04296 * @param[in] time New walk time in milliseconds. 04297 */ 04298 int dmp_set_pedometer_walk_time(unsigned long time) 04299 { 04300 unsigned char tmp[4]; 04301 04302 time /= 20; 04303 04304 tmp[0] = (unsigned char)((time >> 24) & 0xFF); 04305 tmp[1] = (unsigned char)((time >> 16) & 0xFF); 04306 tmp[2] = (unsigned char)((time >> 8) & 0xFF); 04307 tmp[3] = (unsigned char)(time & 0xFF); 04308 return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); 04309 } 04310 04311 /** 04312 * @brief Enable DMP features. 04313 * The following \#define's are used in the input mask: 04314 * \n DMP_FEATURE_TAP 04315 * \n DMP_FEATURE_ANDROID_ORIENT 04316 * \n DMP_FEATURE_LP_QUAT 04317 * \n DMP_FEATURE_6X_LP_QUAT 04318 * \n DMP_FEATURE_GYRO_CAL 04319 * \n DMP_FEATURE_SEND_RAW_ACCEL 04320 * \n DMP_FEATURE_SEND_RAW_GYRO 04321 * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually 04322 * exclusive. 04323 * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also 04324 * mutually exclusive. 04325 * @param[in] mask Mask of features to enable. 04326 * @return 0 if successful. 04327 */ 04328 int dmp_enable_feature(unsigned short mask) 04329 { 04330 unsigned char tmp[10]; 04331 04332 /* TODO: All of these settings can probably be integrated into the default 04333 * DMP image. 04334 */ 04335 /* Set integration scale factor. */ 04336 tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); 04337 tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); 04338 tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); 04339 tmp[3] = (unsigned char)(GYRO_SF & 0xFF); 04340 mpu_write_mem(D_0_104, 4, tmp); 04341 04342 /* Send sensor data to the FIFO. */ 04343 tmp[0] = 0xA3; 04344 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { 04345 tmp[1] = 0xC0; 04346 tmp[2] = 0xC8; 04347 tmp[3] = 0xC2; 04348 } else { 04349 tmp[1] = 0xA3; 04350 tmp[2] = 0xA3; 04351 tmp[3] = 0xA3; 04352 } 04353 if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 04354 tmp[4] = 0xC4; 04355 tmp[5] = 0xCC; 04356 tmp[6] = 0xC6; 04357 } else { 04358 tmp[4] = 0xA3; 04359 tmp[5] = 0xA3; 04360 tmp[6] = 0xA3; 04361 } 04362 tmp[7] = 0xA3; 04363 tmp[8] = 0xA3; 04364 tmp[9] = 0xA3; 04365 mpu_write_mem(CFG_15,10,tmp); 04366 04367 /* Send gesture data to the FIFO. */ 04368 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 04369 tmp[0] = DINA20; 04370 else 04371 tmp[0] = 0xD8; 04372 mpu_write_mem(CFG_27,1,tmp); 04373 04374 if (mask & DMP_FEATURE_GYRO_CAL) 04375 dmp_enable_gyro_cal(1); 04376 else 04377 dmp_enable_gyro_cal(0); 04378 04379 if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 04380 if (mask & DMP_FEATURE_SEND_CAL_GYRO) { 04381 tmp[0] = 0xB2; 04382 tmp[1] = 0x8B; 04383 tmp[2] = 0xB6; 04384 tmp[3] = 0x9B; 04385 } else { 04386 tmp[0] = DINAC0; 04387 tmp[1] = DINA80; 04388 tmp[2] = DINAC2; 04389 tmp[3] = DINA90; 04390 } 04391 mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); 04392 } 04393 04394 if (mask & DMP_FEATURE_TAP) { 04395 /* Enable tap. */ 04396 tmp[0] = 0xF8; 04397 mpu_write_mem(CFG_20, 1, tmp); 04398 dmp_set_tap_thresh(TAP_XYZ, 250); 04399 dmp_set_tap_axes(TAP_XYZ); 04400 dmp_set_tap_count(1); 04401 dmp_set_tap_time(100); 04402 dmp_set_tap_time_multi(500); 04403 04404 dmp_set_shake_reject_thresh(GYRO_SF, 200); 04405 dmp_set_shake_reject_time(40); 04406 dmp_set_shake_reject_timeout(10); 04407 } else { 04408 tmp[0] = 0xD8; 04409 mpu_write_mem(CFG_20, 1, tmp); 04410 } 04411 04412 if (mask & DMP_FEATURE_ANDROID_ORIENT) { 04413 tmp[0] = 0xD9; 04414 } else 04415 tmp[0] = 0xD8; 04416 mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); 04417 04418 if (mask & DMP_FEATURE_LP_QUAT) 04419 dmp_enable_lp_quat(1); 04420 else 04421 dmp_enable_lp_quat(0); 04422 04423 if (mask & DMP_FEATURE_6X_LP_QUAT) 04424 dmp_enable_6x_lp_quat(1); 04425 else 04426 dmp_enable_6x_lp_quat(0); 04427 04428 /* Pedometer is always enabled. */ 04429 dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; 04430 mpu_reset_fifo(); 04431 04432 dmp.packet_length = 0; 04433 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) 04434 dmp.packet_length += 6; 04435 if (mask & DMP_FEATURE_SEND_ANY_GYRO) 04436 dmp.packet_length += 6; 04437 if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) 04438 dmp.packet_length += 16; 04439 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 04440 dmp.packet_length += 4; 04441 04442 return 0; 04443 } 04444 04445 /** 04446 * @brief Get list of currently enabled DMP features. 04447 * @param[out] Mask of enabled features. 04448 * @return 0 if successful. 04449 */ 04450 int dmp_get_enabled_features(unsigned short *mask) 04451 { 04452 mask[0] = dmp.feature_mask; 04453 return 0; 04454 } 04455 04456 /** 04457 * @brief Calibrate the gyro data in the DMP. 04458 * After eight seconds of no motion, the DMP will compute gyro biases and 04459 * subtract them from the quaternion output. If @e dmp_enable_feature is 04460 * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be 04461 * subtracted from the gyro output. 04462 * @param[in] enable 1 to enable gyro calibration. 04463 * @return 0 if successful. 04464 */ 04465 int dmp_enable_gyro_cal(unsigned char enable) 04466 { 04467 if (enable) { 04468 unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; 04469 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 04470 } else { 04471 unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; 04472 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 04473 } 04474 } 04475 04476 /** 04477 * @brief Generate 3-axis quaternions from the DMP. 04478 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 04479 * exclusive. 04480 * @param[in] enable 1 to enable 3-axis quaternion. 04481 * @return 0 if successful. 04482 */ 04483 int dmp_enable_lp_quat(unsigned char enable) 04484 { 04485 unsigned char regs[4]; 04486 if (enable) { 04487 regs[0] = DINBC0; 04488 regs[1] = DINBC2; 04489 regs[2] = DINBC4; 04490 regs[3] = DINBC6; 04491 } 04492 else 04493 memset(regs, 0x8B, 4); 04494 04495 mpu_write_mem(CFG_LP_QUAT, 4, regs); 04496 04497 return mpu_reset_fifo(); 04498 } 04499 04500 /** 04501 * @brief Generate 6-axis quaternions from the DMP. 04502 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 04503 * exclusive. 04504 * @param[in] enable 1 to enable 6-axis quaternion. 04505 * @return 0 if successful. 04506 */ 04507 int dmp_enable_6x_lp_quat(unsigned char enable) 04508 { 04509 unsigned char regs[4]; 04510 if (enable) { 04511 regs[0] = DINA20; 04512 regs[1] = DINA28; 04513 regs[2] = DINA30; 04514 regs[3] = DINA38; 04515 } else 04516 memset(regs, 0xA3, 4); 04517 04518 mpu_write_mem(CFG_8, 4, regs); 04519 04520 return mpu_reset_fifo(); 04521 } 04522 04523 /** 04524 * @brief Decode the four-byte gesture data and execute any callbacks. 04525 * @param[in] gesture Gesture data from DMP packet. 04526 * @return 0 if successful. 04527 */ 04528 static int decode_gesture(unsigned char *gesture) 04529 { 04530 unsigned char tap, android_orient; 04531 04532 android_orient = gesture[3] & 0xC0; 04533 tap = 0x3F & gesture[3]; 04534 04535 if (gesture[1] & INT_SRC_TAP) { 04536 unsigned char direction, count; 04537 direction = tap >> 3; 04538 count = (tap % 8) + 1; 04539 if (dmp.tap_cb) 04540 dmp.tap_cb(direction, count); 04541 } 04542 04543 if (gesture[1] & INT_SRC_ANDROID_ORIENT) { 04544 if (dmp.android_orient_cb) 04545 dmp.android_orient_cb(android_orient >> 6); 04546 } 04547 04548 return 0; 04549 } 04550 04551 /** 04552 * @brief Specify when a DMP interrupt should occur. 04553 * A DMP interrupt can be configured to trigger on either of the two 04554 * conditions below: 04555 * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). 04556 * \n b. A tap event has been detected. 04557 * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. 04558 * @return 0 if successful. 04559 */ 04560 int dmp_set_interrupt_mode(unsigned char mode) 04561 { 04562 const unsigned char regs_continuous[11] = 04563 {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; 04564 const unsigned char regs_gesture[11] = 04565 {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; 04566 04567 switch (mode) { 04568 case DMP_INT_CONTINUOUS: 04569 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 04570 (unsigned char*)regs_continuous); 04571 case DMP_INT_GESTURE: 04572 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 04573 (unsigned char*)regs_gesture); 04574 default: 04575 return -1; 04576 } 04577 } 04578 04579 /** 04580 * @brief Get one packet from the FIFO. 04581 * If @e sensors does not contain a particular sensor, disregard the data 04582 * returned to that pointer. 04583 * \n @e sensors can contain a combination of the following flags: 04584 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 04585 * \n INV_XYZ_GYRO 04586 * \n INV_XYZ_ACCEL 04587 * \n INV_WXYZ_QUAT 04588 * \n If the FIFO has no new data, @e sensors will be zero. 04589 * \n If the FIFO is disabled, @e sensors will be zero and this function will 04590 * return a non-zero error code. 04591 * @param[out] gyro Gyro data in hardware units. 04592 * @param[out] accel Accel data in hardware units. 04593 * @param[out] quat 3-axis quaternion data in hardware units. 04594 * @param[out] timestamp Timestamp in milliseconds. 04595 * @param[out] sensors Mask of sensors read from FIFO. 04596 * @param[out] more Number of remaining packets. 04597 * @return 0 if successful. 04598 */ 04599 int dmp_read_fifo(short *gyro, short *accel, long *quat, 04600 unsigned long *timestamp, short *sensors, unsigned char *more) 04601 { 04602 unsigned char fifo_data[MAX_PACKET_LENGTH_2]; 04603 unsigned char ii = 0; 04604 04605 /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can 04606 * cache this value and save some cycles. 04607 */ 04608 sensors[0] = 0; 04609 04610 /* Get a packet. */ 04611 if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) 04612 return -1; 04613 04614 /* Parse DMP packet. */ 04615 if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { 04616 #ifdef FIFO_CORRUPTION_CHECK 04617 long quat_q14[4], quat_mag_sq; 04618 #endif 04619 quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | 04620 ((long)fifo_data[2] << 8) | fifo_data[3]; 04621 quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | 04622 ((long)fifo_data[6] << 8) | fifo_data[7]; 04623 quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | 04624 ((long)fifo_data[10] << 8) | fifo_data[11]; 04625 quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | 04626 ((long)fifo_data[14] << 8) | fifo_data[15]; 04627 ii += 16; 04628 #ifdef FIFO_CORRUPTION_CHECK 04629 /* We can detect a corrupted FIFO by monitoring the quaternion data and 04630 * ensuring that the magnitude is always normalized to one. This 04631 * shouldn't happen in normal operation, but if an I2C error occurs, 04632 * the FIFO reads might become misaligned. 04633 * 04634 * Let's start by scaling down the quaternion data to avoid long long 04635 * math. 04636 */ 04637 quat_q14[0] = quat[0] >> 16; 04638 quat_q14[1] = quat[1] >> 16; 04639 quat_q14[2] = quat[2] >> 16; 04640 quat_q14[3] = quat[3] >> 16; 04641 quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + 04642 quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; 04643 if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || 04644 (quat_mag_sq > QUAT_MAG_SQ_MAX)) { 04645 /* Quaternion is outside of the acceptable threshold. */ 04646 mpu_reset_fifo(); 04647 sensors[0] = 0; 04648 return -1; 04649 } 04650 sensors[0] |= INV_WXYZ_QUAT; 04651 #endif 04652 } 04653 04654 if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { 04655 accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 04656 accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 04657 accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 04658 ii += 6; 04659 sensors[0] |= INV_XYZ_ACCEL; 04660 } 04661 04662 if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { 04663 gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 04664 gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 04665 gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 04666 ii += 6; 04667 sensors[0] |= INV_XYZ_GYRO; 04668 } 04669 04670 /* Gesture data is at the end of the DMP packet. Parse it and call 04671 * the gesture callbacks (if registered). 04672 */ 04673 if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 04674 decode_gesture(fifo_data + ii); 04675 04676 get_ms(timestamp); 04677 return 0; 04678 } 04679 04680 /** 04681 * @brief Register a function to be executed on a tap event. 04682 * The tap direction is represented by one of the following: 04683 * \n TAP_X_UP 04684 * \n TAP_X_DOWN 04685 * \n TAP_Y_UP 04686 * \n TAP_Y_DOWN 04687 * \n TAP_Z_UP 04688 * \n TAP_Z_DOWN 04689 * @param[in] func Callback function. 04690 * @return 0 if successful. 04691 */ 04692 int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) 04693 { 04694 dmp.tap_cb = func; 04695 return 0; 04696 } 04697 04698 /** 04699 * @brief Register a function to be executed on a android orientation event. 04700 * @param[in] func Callback function. 04701 * @return 0 if successful. 04702 */ 04703 int dmp_register_android_orient_cb(void (*func)(unsigned char)) 04704 { 04705 dmp.android_orient_cb = func; 04706 return 0; 04707 }
Generated on Sat Jul 16 2022 18:13:26 by
1.7.2