Keegan Hu
控制平衡车的代码
Fork of
MPU6050_Driver_Balance
by 
Keegan Hu
Diff: mpu6050.cpp
- Revision:
- 0:badebd32bd8b
diff -r 000000000000 -r badebd32bd8b mpu6050.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mpu6050.cpp Mon Apr 09 14:34:45 2018 +0000
@@ -0,0 +1,4707 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include "mpu6050.h"
+
+//////////////////////////////////////////////////////////////////////////////////
+//MPU6050驱动程序 C.H.
+//////////////////////////////////////////////////////////////////////////////////
+
+
+
+//初始化MPU6050
+//返回值:0,成功
+// 其他,错误代码
+unsigned char MPU_Init(void)
+{
+ unsigned char res;
+ MPU_IIC_Init();//初始化IIC总线
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
+ delay_ms(100);
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
+ MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
+ MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
+ MPU_Set_Rate(50); //设置采样率50Hz
+ MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
+ MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
+ MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
+ MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
+ res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
+ if(res==MPU_ADDR)//器件ID正确
+ {
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
+ MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
+ MPU_Set_Rate(50); //设置采样率为50Hz
+ }else return 1;
+ return 0;
+}
+
+
+
+//设置MPU6050陀螺仪传感器满量程范围
+//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
+//返回值:0,设置成功
+// 其他,设置失败
+unsigned char MPU_Set_Gyro_Fsr(unsigned char fsr)
+{
+ return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
+}
+//设置MPU6050加速度传感器满量程范围
+//fsr:0,±2g;1,±4g;2,±8g;3,±16g
+//返回值:0,设置成功
+// 其他,设置失败
+unsigned char MPU_Set_Accel_Fsr(unsigned char fsr)
+{
+ return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
+}
+//设置MPU6050的数字低通滤波器
+//lpf:数字低通滤波频率(Hz)
+//返回值:0,设置成功
+// 其他,设置失败
+unsigned char MPU_Set_LPF(unsigned short lpf)
+{
+ unsigned char data=0;
+ if(lpf>=188)data=1;
+ else if(lpf>=98)data=2;
+ else if(lpf>=42)data=3;
+ else if(lpf>=20)data=4;
+ else if(lpf>=10)data=5;
+ else data=6;
+ return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
+}
+//设置MPU6050的采样率(假定Fs=1KHz)
+//rate:4~1000(Hz)
+//返回值:0,设置成功
+// 其他,设置失败
+unsigned char MPU_Set_Rate(unsigned short rate)
+{
+ unsigned char data;
+ if(rate>1000)rate=1000;
+ if(rate<4)rate=4;
+ data=1000/rate-1;
+ data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
+ return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
+}
+
+//得到温度值
+//返回值:温度值(扩大了100倍)
+short MPU_Get_Temperature(void)
+{
+ unsigned char buf[2];
+ short raw;
+ float temp;
+ MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
+ raw=((unsigned short )buf[0]<<8)|buf[1];
+ temp=36.53+((double)raw)/340;
+ return temp*100;;
+}
+//得到陀螺仪值(原始值)
+//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
+//返回值:0,成功
+// 其他,错误代码
+unsigned char MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
+{
+ unsigned char buf[6],res;
+ res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
+ if(res==0)
+ {
+ *gx=((unsigned short )buf[0]<<8)|buf[1];
+ *gy=((unsigned short )buf[2]<<8)|buf[3];
+ *gz=((unsigned short )buf[4]<<8)|buf[5];
+ }
+ return res;;
+}
+//得到加速度值(原始值)
+//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
+//返回值:0,成功
+// 其他,错误代码
+unsigned char MPU_Get_Accelerometer(short *ax,short *ay,short *az)
+{
+ unsigned char buf[6],res;
+ res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
+ if(res==0)
+ {
+ *ax=((unsigned short )buf[0]<<8)|buf[1];
+ *ay=((unsigned short )buf[2]<<8)|buf[3];
+ *az=((unsigned short )buf[4]<<8)|buf[5];
+ }
+ return res;;
+}
+//IIC连续写
+//addr:器件地址
+//reg:寄存器地址
+//len:写入长度
+//buf:数据区
+//返回值:0,正常
+// 其他,错误代码
+unsigned char MPU_Write_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
+{
+ unsigned char i;
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ for(i=0;i<len;i++)
+ {
+ MPU_IIC_Send_Byte(buf[i]); //发送数据
+ if(MPU_IIC_Wait_Ack()) //等待ACK
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ }
+ MPU_IIC_Stop();
+ return 0;
+}
+//IIC连续读
+//addr:器件地址
+//reg:要读取的寄存器地址
+//len:要读取的长度
+//buf:读取到的数据存储区
+//返回值:0,正常
+// 其他,错误代码
+unsigned char MPU_Read_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
+{
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ while(len)
+ {
+ if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
+ else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
+ len--;
+ buf++;
+ }
+ MPU_IIC_Stop(); //产生一个停止条件
+ return 0;
+}
+//IIC写一个字节
+//reg:寄存器地址
+//data:数据
+//返回值:0,正常
+// 其他,错误代码
+unsigned char MPU_Write_Byte(unsigned char reg,unsigned char data)
+{
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Send_Byte(data);//发送数据
+ if(MPU_IIC_Wait_Ack()) //等待ACK
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Stop();
+ return 0;
+}
+//IIC读一个字节
+//reg:寄存器地址
+//返回值:读到的数据
+unsigned char MPU_Read_Byte(unsigned char reg)
+{
+ unsigned char res;
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
+ MPU_IIC_Stop(); //产生一个停止条件
+ return res;
+}/**/
+
+
+//MPU IIC 延时函数
+void MPU_IIC_Delay(void)
+{
+ delay_us(2);
+}
+//初始化IIC
+void MPU_IIC_Init(void)
+{
+
+
+#if defined DRIVER_MODE_BALANCE
+ RCC->APB2ENR|=1<<3; //先使能外设IO PORTC时钟
+ GPIOB->CRL&=0X00FFFFFF; //PC11/12 推挽输出
+ GPIOB->CRL|=0X33000000;
+ GPIOB->ODR|=3<<6; //PC11,12 输出高
+#elif defined DRIVER_MODE_ROTOR
+ RCC->APB2ENR|=1<<3; //先使能外设IO PORTC时钟
+ GPIOB->CRH&=0XFFF00FFF; //PC11/12 推挽输出
+ GPIOB->CRH|=0X00033000;
+ GPIOB->ODR|=3<<11; //PC11,12 输出高
+#else
+#error Target Board is not specified.
+#endif
+}
+//产生IIC起始信号
+void MPU_IIC_Start(void)
+{
+ MPU_SDA_OUT(); //sda线输出
+ MPU_IIC_SDA=1;
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
+}
+//产生IIC停止信号
+void MPU_IIC_Stop(void)
+{
+ MPU_SDA_OUT();//sda线输出
+ MPU_IIC_SCL=0;
+ MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_SDA=1;//发送I2C总线结束信号
+ MPU_IIC_Delay();
+}
+//等待应答信号到来
+//返回值：1，接收应答失败
+// 0，接收应答成功
+unsigned char MPU_IIC_Wait_Ack(void)
+{
+ unsigned char ucErrTime=0;
+ MPU_SDA_IN(); //SDA设置为输入
+ MPU_IIC_SDA=1;MPU_IIC_Delay();
+ MPU_IIC_SCL=1;MPU_IIC_Delay();
+ while(MPU_READ_SDA)
+ {
+ ucErrTime++;
+ if(ucErrTime>250)
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ }
+ MPU_IIC_SCL=0;//时钟输出0
+ return 0;
+}
+//产生ACK应答
+void MPU_IIC_Ack(void)
+{
+ MPU_IIC_SCL=0;
+ MPU_SDA_OUT();
+ MPU_IIC_SDA=0;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+}
+//不产生ACK应答
+void MPU_IIC_NAck(void)
+{
+ MPU_IIC_SCL=0;
+ MPU_SDA_OUT();
+ MPU_IIC_SDA=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+}
+//IIC发送一个字节
+//返回从机有无应答
+//1，有应答
+//0，无应答
+void MPU_IIC_Send_Byte(unsigned char txd)
+{
+ unsigned char t;
+ MPU_SDA_OUT();
+ MPU_IIC_SCL=0;//拉低时钟开始数据传输
+ for(t=0;t<8;t++)
+ {
+ MPU_IIC_SDA=(txd&0x80)>>7;
+ txd<<=1;
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+ MPU_IIC_Delay();
+ }
+}
+//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
+unsigned char MPU_IIC_Read_Byte(unsigned char ack)
+{
+ unsigned char i,receive=0;
+ MPU_SDA_IN();//SDA设置为输入
+ for(i=0;i<8;i++ )
+ {
+ MPU_IIC_SCL=0;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ receive<<=1;
+ if(MPU_READ_SDA)receive++;
+ MPU_IIC_Delay();
+ }
+ if (!ack)
+ MPU_IIC_NAck();//发送nACK
+ else
+ MPU_IIC_Ack(); //发送ACK
+ return receive;
+}
+
+#define MPU6050 //定义我们使用的传感器为MPU6050
+#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1)
+
+/* The following functions must be defined for this platform:
+ * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
+ * unsigned char length, unsigned char const *data)
+ * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
+ * unsigned char length, unsigned char *data)
+ * delay_ms(unsigned long num_ms)
+ * get_ms(unsigned long *count)
+ * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
+ * labs(long x)
+ * fabsf(float x)
+ * min(int a, int b)
+ */
+#if defined MOTION_DRIVER_TARGET_MSP430
+//#include "msp430.h"
+//#include "msp430_i2c.h"
+//#include "msp430_clock.h"
+//#include "msp430_interrupt.h"
+
+#define i2c_write MPU_Write_Len
+#define i2c_read MPU_Read_Len
+//#define delay_ms delay_ms
+#define get_ms mget_ms
+//static inline int reg_int_cb(struct int_param_s *int_param)
+//{
+// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
+// int_param->active_low);
+//}
+////#define // printf //打印信息
+////#define // printf //打印信息
+/* labs is already defined by TI's toolchain. */
+/* fabs is for doubles. fabsf is for floats. */
+#define fabs fabsf
+#define min(a,b) ((a<b)?a:b)
+#elif defined EMPL_TARGET_MSP430
+#include "msp430.h"
+#include "msp430_i2c.h"
+#include "msp430_clock.h"
+#include "msp430_interrupt.h"
+#include "log.h"
+#define i2c_write msp430_i2c_write
+#define i2c_read msp430_i2c_read
+#define delay_ms msp430_delay_ms
+#define get_ms msp430_get_clock_ms
+static inline int reg_int_cb(struct int_param_s *int_param)
+{
+ return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
+ int_param->active_low);
+}
+//#define // MPL_LOGI
+//#define // MPL_LOGE
+/* labs is already defined by TI's toolchain. */
+/* fabs is for doubles. fabsf is for floats. */
+#define fabs fabsf
+#define min(a,b) ((a<b)?a:b)
+#elif defined EMPL_TARGET_UC3L0
+/* Instead of using the standard TWI driver from the ASF library, we're using
+ * a TWI driver that follows the slave address + register address convention.
+ */
+#include "twi.h"
+#include "delay.h"
+#include "sysclk.h"
+#include "log.h"
+#include "sensors_xplained.h"
+#include "uc3l0_clock.h"
+#define i2c_write(a, b, c, d) twi_write(a, b, d, c)
+#define i2c_read(a, b, c, d) twi_read(a, b, d, c)
+/* delay_ms is a function already defined in ASF. */
+#define get_ms uc3l0_get_clock_ms
+static inline int reg_int_cb(struct int_param_s *int_param)
+{
+ sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
+ return 0;
+}
+//#define // MPL_LOGI
+//#define // MPL_LOGE
+/* UC3 is a 32-bit processor, so abs and labs are equivalent. */
+#define labs abs
+#define fabs(x) (((x)>0)?(x):-(x))
+#else
+#error Gyro driver is missing the system layer implementations.
+#endif
+
+#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
+#error Which gyro are you using? Define MPUxxxx in your compiler options.
+#endif
+
+/* Time for some messy macro work. =]
+ * #define MPU9150
+ * is equivalent to..
+ * #define MPU6050
+ * #define AK8975_SECONDARY
+ *
+ * #define MPU9250
+ * is equivalent to..
+ * #define MPU6500
+ * #define AK8963_SECONDARY
+ */
+#if defined MPU9150
+#ifndef MPU6050
+#define MPU6050
+#endif /* #ifndef MPU6050 */
+#if defined AK8963_SECONDARY
+#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
+#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
+#define AK8975_SECONDARY
+#endif /* #if defined AK8963_SECONDARY */
+#elif defined MPU9250 /* #if defined MPU9150 */
+#ifndef MPU6500
+#define MPU6500
+#endif /* #ifndef MPU6500 */
+#if defined AK8975_SECONDARY
+#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
+#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
+#define AK8963_SECONDARY
+#endif /* #if defined AK8975_SECONDARY */
+#endif /* #if defined MPU9150 */
+
+#if defined AK8975_SECONDARY || defined AK8963_SECONDARY
+#define AK89xx_SECONDARY
+#else
+/* #warning "No compass = less profit for Invensense. Lame." */
+#endif
+
+static int set_int_enable(unsigned char enable);
+
+/* Hardware registers needed by driver. */
+struct gyro_reg_s {
+ unsigned char who_am_i;
+ unsigned char rate_div;
+ unsigned char lpf;
+ unsigned char prod_id;
+ unsigned char user_ctrl;
+ unsigned char fifo_en;
+ unsigned char gyro_cfg;
+ unsigned char accel_cfg;
+// unsigned char accel_cfg2;
+// unsigned char lp_accel_odr;
+ unsigned char motion_thr;
+ unsigned char motion_dur;
+ unsigned char fifo_count_h;
+ unsigned char fifo_r_w;
+ unsigned char raw_gyro;
+ unsigned char raw_accel;
+ unsigned char temp;
+ unsigned char int_enable;
+ unsigned char dmp_int_status;
+ unsigned char int_status;
+// unsigned char accel_intel;
+ unsigned char pwr_mgmt_1;
+ unsigned char pwr_mgmt_2;
+ unsigned char int_pin_cfg;
+ unsigned char mem_r_w;
+ unsigned char accel_offs;
+ unsigned char i2c_mst;
+ unsigned char bank_sel;
+ unsigned char mem_start_addr;
+ unsigned char prgm_start_h;
+#if defined AK89xx_SECONDARY
+ unsigned char s0_addr;
+ unsigned char s0_reg;
+ unsigned char s0_ctrl;
+ unsigned char s1_addr;
+ unsigned char s1_reg;
+ unsigned char s1_ctrl;
+ unsigned char s4_ctrl;
+ unsigned char s0_do;
+ unsigned char s1_do;
+ unsigned char i2c_delay_ctrl;
+ unsigned char raw_compass;
+ /* The I2C_MST_VDDIO bit is in this register. */
+ unsigned char yg_offs_tc;
+#endif
+};
+
+/* Information specific to a particular device. */
+struct hw_s {
+ unsigned char addr;
+ unsigned short max_fifo;
+ unsigned char num_reg;
+ unsigned short temp_sens;
+ short temp_offset;
+ unsigned short bank_size;
+#if defined AK89xx_SECONDARY
+ unsigned short compass_fsr;
+#endif
+};
+
+/* When entering motion interrupt mode, the driver keeps track of the
+ * previous state so that it can be restored at a later time.
+ * TODO: This is tacky. Fix it.
+ */
+struct motion_int_cache_s {
+ unsigned short gyro_fsr;
+ unsigned char accel_fsr;
+ unsigned short lpf;
+ unsigned short sample_rate;
+ unsigned char sensors_on;
+ unsigned char fifo_sensors;
+ unsigned char dmp_on;
+};
+
+/* Cached chip configuration data.
+ * TODO: A lot of these can be handled with a bitmask.
+ */
+struct chip_cfg_s {
+ /* Matches gyro_cfg >> 3 & 0x03 */
+ unsigned char gyro_fsr;
+ /* Matches accel_cfg >> 3 & 0x03 */
+ unsigned char accel_fsr;
+ /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
+ unsigned char sensors;
+ /* Matches config register. */
+ unsigned char lpf;
+ unsigned char clk_src;
+ /* Sample rate, NOT rate divider. */
+ unsigned short sample_rate;
+ /* Matches fifo_en register. */
+ unsigned char fifo_enable;
+ /* Matches int enable register. */
+ unsigned char int_enable;
+ /* 1 if devices on auxiliary I2C bus appear on the primary. */
+ unsigned char bypass_mode;
+ /* 1 if half-sensitivity.
+ * NOTE: This doesn't belong here, but everything else in hw_s is const,
+ * and this allows us to save some precious RAM.
+ */
+ unsigned char accel_half;
+ /* 1 if device in low-power accel-only mode. */
+ unsigned char lp_accel_mode;
+ /* 1 if interrupts are only triggered on motion events. */
+ unsigned char int_motion_only;
+ struct motion_int_cache_s cache;
+ /* 1 for active low interrupts. */
+ unsigned char active_low_int;
+ /* 1 for latched interrupts. */
+ unsigned char latched_int;
+ /* 1 if DMP is enabled. */
+ unsigned char dmp_on;
+ /* Ensures that DMP will only be loaded once. */
+ unsigned char dmp_loaded;
+ /* Sampling rate used when DMP is enabled. */
+ unsigned short dmp_sample_rate;
+#ifdef AK89xx_SECONDARY
+ /* Compass sample rate. */
+ unsigned short compass_sample_rate;
+ unsigned char compass_addr;
+ short mag_sens_adj[3];
+#endif
+};
+
+/* Information for self-test. */
+struct test_s {
+ unsigned long gyro_sens;
+ unsigned long accel_sens;
+ unsigned char reg_rate_div;
+ unsigned char reg_lpf;
+ unsigned char reg_gyro_fsr;
+ unsigned char reg_accel_fsr;
+ unsigned short wait_ms;
+ unsigned char packet_thresh;
+ float min_dps;
+ float max_dps;
+ float max_gyro_var;
+ float min_g;
+ float max_g;
+ float max_accel_var;
+};
+
+/* Gyro driver state variables. */
+struct gyro_state_s {
+ const struct gyro_reg_s *reg;
+ const struct hw_s *hw;
+ struct chip_cfg_s chip_cfg;
+ const struct test_s *test;
+};
+
+/* Filter configurations. */
+enum lpf_e {
+ INV_FILTER_256HZ_NOLPF2 = 0,
+ INV_FILTER_188HZ,
+ INV_FILTER_98HZ,
+ INV_FILTER_42HZ,
+ INV_FILTER_20HZ,
+ INV_FILTER_10HZ,
+ INV_FILTER_5HZ,
+ INV_FILTER_2100HZ_NOLPF,
+ NUM_FILTER
+};
+
+/* Full scale ranges. */
+enum gyro_fsr_e {
+ INV_FSR_250DPS = 0,
+ INV_FSR_500DPS,
+ INV_FSR_1000DPS,
+ INV_FSR_2000DPS,
+ NUM_GYRO_FSR
+};
+
+/* Full scale ranges. */
+enum accel_fsr_e {
+ INV_FSR_2G = 0,
+ INV_FSR_4G,
+ INV_FSR_8G,
+ INV_FSR_16G,
+ NUM_ACCEL_FSR
+};
+
+/* Clock sources. */
+enum clock_sel_e {
+ INV_CLK_INTERNAL = 0,
+ INV_CLK_PLL,
+ NUM_CLK
+};
+
+/* Low-power accel wakeup rates. */
+enum lp_accel_rate_e {
+#if defined MPU6050
+ INV_LPA_1_25HZ,
+ INV_LPA_5HZ,
+ INV_LPA_20HZ,
+ INV_LPA_40HZ
+#elif defined MPU6500
+ INV_LPA_0_3125HZ,
+ INV_LPA_0_625HZ,
+ INV_LPA_1_25HZ,
+ INV_LPA_2_5HZ,
+ INV_LPA_5HZ,
+ INV_LPA_10HZ,
+ INV_LPA_20HZ,
+ INV_LPA_40HZ,
+ INV_LPA_80HZ,
+ INV_LPA_160HZ,
+ INV_LPA_320HZ,
+ INV_LPA_640HZ
+#endif
+};
+
+#define BIT_I2C_MST_VDDIO (0x80)
+#define BIT_FIFO_EN (0x40)
+#define BIT_DMP_EN (0x80)
+#define BIT_FIFO_RST (0x04)
+#define BIT_DMP_RST (0x08)
+#define BIT_FIFO_OVERFLOW (0x10)
+#define BIT_DATA_RDY_EN (0x01)
+#define BIT_DMP_INT_EN (0x02)
+#define BIT_MOT_INT_EN (0x40)
+#define BITS_FSR (0x18)
+#define BITS_LPF (0x07)
+#define BITS_HPF (0x07)
+#define BITS_CLK (0x07)
+#define BIT_FIFO_SIZE_1024 (0x40)
+#define BIT_FIFO_SIZE_2048 (0x80)
+#define BIT_FIFO_SIZE_4096 (0xC0)
+#define BIT_RESET (0x80)
+#define BIT_SLEEP (0x40)
+#define BIT_S0_DELAY_EN (0x01)
+#define BIT_S2_DELAY_EN (0x04)
+#define BITS_SLAVE_LENGTH (0x0F)
+#define BIT_SLAVE_BYTE_SW (0x40)
+#define BIT_SLAVE_GROUP (0x10)
+#define BIT_SLAVE_EN (0x80)
+#define BIT_I2C_READ (0x80)
+#define BITS_I2C_MASTER_DLY (0x1F)
+#define BIT_AUX_IF_EN (0x20)
+#define BIT_ACTL (0x80)
+#define BIT_LATCH_EN (0x20)
+#define BIT_ANY_RD_CLR (0x10)
+#define BIT_BYPASS_EN (0x02)
+#define BITS_WOM_EN (0xC0)
+#define BIT_LPA_CYCLE (0x20)
+#define BIT_STBY_XA (0x20)
+#define BIT_STBY_YA (0x10)
+#define BIT_STBY_ZA (0x08)
+#define BIT_STBY_XG (0x04)
+#define BIT_STBY_YG (0x02)
+#define BIT_STBY_ZG (0x01)
+#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
+#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
+
+#if defined AK8975_SECONDARY
+#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
+#define AK89xx_FSR (9830)
+#elif defined AK8963_SECONDARY
+#define SUPPORTS_AK89xx_HIGH_SENS (0x10)
+#define AK89xx_FSR (4915)
+#endif
+
+#ifdef AK89xx_SECONDARY
+#define AKM_REG_WHOAMI (0x00)
+
+#define AKM_REG_ST1 (0x02)
+#define AKM_REG_HXL (0x03)
+#define AKM_REG_ST2 (0x09)
+
+#define AKM_REG_CNTL (0x0A)
+#define AKM_REG_ASTC (0x0C)
+#define AKM_REG_ASAX (0x10)
+#define AKM_REG_ASAY (0x11)
+#define AKM_REG_ASAZ (0x12)
+
+#define AKM_DATA_READY (0x01)
+#define AKM_DATA_OVERRUN (0x02)
+#define AKM_OVERFLOW (0x80)
+#define AKM_DATA_ERROR (0x40)
+
+#define AKM_BIT_SELF_TEST (0x40)
+
+#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
+#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
+#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
+#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
+
+#define AKM_WHOAMI (0x48)
+#endif
+
+#if defined MPU6050
+//const struct gyro_reg_s reg = {
+// .who_am_i = 0x75,
+// .rate_div = 0x19,
+// .lpf = 0x1A,
+// .prod_id = 0x0C,
+// .user_ctrl = 0x6A,
+// .fifo_en = 0x23,
+// .gyro_cfg = 0x1B,
+// .accel_cfg = 0x1C,
+// .motion_thr = 0x1F,
+// .motion_dur = 0x20,
+// .fifo_count_h = 0x72,
+// .fifo_r_w = 0x74,
+// .raw_gyro = 0x43,
+// .raw_accel = 0x3B,
+// .temp = 0x41,
+// .int_enable = 0x38,
+// .dmp_int_status = 0x39,
+// .int_status = 0x3A,
+// .pwr_mgmt_1 = 0x6B,
+// .pwr_mgmt_2 = 0x6C,
+// .int_pin_cfg = 0x37,
+// .mem_r_w = 0x6F,
+// .accel_offs = 0x06,
+// .i2c_mst = 0x24,
+// .bank_sel = 0x6D,
+// .mem_start_addr = 0x6E,
+// .prgm_start_h = 0x70
+//#ifdef AK89xx_SECONDARY
+// ,.raw_compass = 0x49,
+// .yg_offs_tc = 0x01,
+// .s0_addr = 0x25,
+// .s0_reg = 0x26,
+// .s0_ctrl = 0x27,
+// .s1_addr = 0x28,
+// .s1_reg = 0x29,
+// .s1_ctrl = 0x2A,
+// .s4_ctrl = 0x34,
+// .s0_do = 0x63,
+// .s1_do = 0x64,
+// .i2c_delay_ctrl = 0x67
+//#endif
+//};
+const struct gyro_reg_s reg = {
+0x75, //who_am_i
+0x19, //rate_div
+0x1A, //lpf
+0x0C, //prod_id
+0x6A, //user_ctrl
+0x23, //fifo_en
+0x1B, //gyro_cfg
+0x1C, //accel_cfg
+0x1F, // motion_thr
+0x20, // motion_dur
+0x72, // fifo_count_h
+0x74, // fifo_r_w
+0x43, // raw_gyro
+0x3B, // raw_accel
+0x41, // temp
+0x38, // int_enable
+0x39, // dmp_int_status
+0x3A, // int_status
+0x6B, // pwr_mgmt_1
+0x6C, // pwr_mgmt_2
+0x37, // int_pin_cfg
+0x6F, // mem_r_w
+0x06, // accel_offs
+0x24, // i2c_mst
+0x6D, // bank_sel
+0x6E, // mem_start_addr
+0x70 // prgm_start_h
+};
+
+//const struct hw_s hw = {
+// .addr = 0x68,
+// .max_fifo = 1024,
+// .num_reg = 118,
+// .temp_sens = 340,
+// .temp_offset = -521,
+// .bank_size = 256
+//#if defined AK89xx_SECONDARY
+// ,.compass_fsr = AK89xx_FSR
+//#endif
+//};
+const struct hw_s hw={
+ 0x68, //addr
+ 1024, //max_fifo
+ 118, //num_reg
+ 340, //temp_sens
+ -521, //temp_offset
+ 256 //bank_size
+};
+
+//const struct test_s test = {
+// .gyro_sens = 32768/250,
+// .accel_sens = 32768/16,
+// .reg_rate_div = 0, /* 1kHz. */
+// .reg_lpf = 1, /* 188Hz. */
+// .reg_gyro_fsr = 0, /* 250dps. */
+// .reg_accel_fsr = 0x18, /* 16g. */
+// .wait_ms = 50,
+// .packet_thresh = 5, /* 5% */
+// .min_dps = 10.f,
+// .max_dps = 105.f,
+// .max_gyro_var = 0.14f,
+// .min_g = 0.3f,
+// .max_g = 0.95f,
+// .max_accel_var = 0.14f
+//};
+const struct test_s test={
+32768/250, //gyro_sens
+32768/16, // accel_sens
+0, // reg_rate_div
+1, // reg_lpf
+0, // reg_gyro_fsr
+0x18, // reg_accel_fsr
+50, // wait_ms
+5, // packet_thresh
+10.0f, // min_dps
+105.0f, // max_dps
+0.14f, // max_gyro_var
+0.3f, // min_g
+0.95f, // max_g
+0.14f // max_accel_var
+};
+
+//static struct gyro_state_s st = {
+// .reg = ®,
+// .hw = &hw,
+// .test = &test
+//};
+static struct gyro_state_s st={
+ ®,
+ &hw,
+ {0},
+ &test
+};
+
+
+#elif defined MPU6500
+const struct gyro_reg_s reg = {
+ .who_am_i = 0x75,
+ .rate_div = 0x19,
+ .lpf = 0x1A,
+ .prod_id = 0x0C,
+ .user_ctrl = 0x6A,
+ .fifo_en = 0x23,
+ .gyro_cfg = 0x1B,
+ .accel_cfg = 0x1C,
+ .accel_cfg2 = 0x1D,
+ .lp_accel_odr = 0x1E,
+ .motion_thr = 0x1F,
+ .motion_dur = 0x20,
+ .fifo_count_h = 0x72,
+ .fifo_r_w = 0x74,
+ .raw_gyro = 0x43,
+ .raw_accel = 0x3B,
+ .temp = 0x41,
+ .int_enable = 0x38,
+ .dmp_int_status = 0x39,
+ .int_status = 0x3A,
+ .accel_intel = 0x69,
+ .pwr_mgmt_1 = 0x6B,
+ .pwr_mgmt_2 = 0x6C,
+ .int_pin_cfg = 0x37,
+ .mem_r_w = 0x6F,
+ .accel_offs = 0x77,
+ .i2c_mst = 0x24,
+ .bank_sel = 0x6D,
+ .mem_start_addr = 0x6E,
+ .prgm_start_h = 0x70
+#ifdef AK89xx_SECONDARY
+ ,.raw_compass = 0x49,
+ .s0_addr = 0x25,
+ .s0_reg = 0x26,
+ .s0_ctrl = 0x27,
+ .s1_addr = 0x28,
+ .s1_reg = 0x29,
+ .s1_ctrl = 0x2A,
+ .s4_ctrl = 0x34,
+ .s0_do = 0x63,
+ .s1_do = 0x64,
+ .i2c_delay_ctrl = 0x67
+#endif
+};
+const struct hw_s hw = {
+ .addr = 0x68,
+ .max_fifo = 1024,
+ .num_reg = 128,
+ .temp_sens = 321,
+ .temp_offset = 0,
+ .bank_size = 256
+#if defined AK89xx_SECONDARY
+ ,.compass_fsr = AK89xx_FSR
+#endif
+};
+
+const struct test_s test = {
+ .gyro_sens = 32768/250,
+ .accel_sens = 32768/16,
+ .reg_rate_div = 0, /* 1kHz. */
+ .reg_lpf = 1, /* 188Hz. */
+ .reg_gyro_fsr = 0, /* 250dps. */
+ .reg_accel_fsr = 0x18, /* 16g. */
+ .wait_ms = 50,
+ .packet_thresh = 5, /* 5% */
+ .min_dps = 10.f,
+ .max_dps = 105.f,
+ .max_gyro_var = 0.14f,
+ .min_g = 0.3f,
+ .max_g = 0.95f,
+ .max_accel_var = 0.14f
+};
+
+static struct gyro_state_s st = {
+ .reg = ®,
+ .hw = &hw,
+ .test = &test
+};
+#endif
+
+#define MAX_PACKET_LENGTH (12)
+
+#ifdef AK89xx_SECONDARY
+static int setup_compass(void);
+#define MAX_COMPASS_SAMPLE_RATE (100)
+#endif
+
+/**
+ * @brief Enable/disable data ready interrupt.
+ * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
+ * interrupt is used.
+ * @param[in] enable 1 to enable interrupt.
+ * @return 0 if successful.
+ */
+static int set_int_enable(unsigned char enable)
+{
+ unsigned char tmp;
+
+ if (st.chip_cfg.dmp_on) {
+ if (enable)
+ tmp = BIT_DMP_INT_EN;
+ else
+ tmp = 0x00;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
+ return -1;
+ st.chip_cfg.int_enable = tmp;
+ } else {
+ if (!st.chip_cfg.sensors)
+ return -1;
+ if (enable && st.chip_cfg.int_enable)
+ return 0;
+ if (enable)
+ tmp = BIT_DATA_RDY_EN;
+ else
+ tmp = 0x00;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
+ return -1;
+ st.chip_cfg.int_enable = tmp;
+ }
+ return 0;
+}
+
+/**
+ * @brief Register dump for testing.
+ * @return 0 if successful.
+ */
+int mpu_reg_dump(void)
+{
+ unsigned char ii;
+ unsigned char data;
+
+ for (ii = 0; ii < st.hw->num_reg; ii++) {
+ if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
+ continue;
+ if (i2c_read(st.hw->addr, ii, 1, &data))
+ return -1;
+ //("%#5x: %#5x\r\n", ii, data);
+ }
+ return 0;
+}
+
+/**
+ * @brief Read from a single register.
+ * NOTE: The memory and FIFO read/write registers cannot be accessed.
+ * @param[in] reg Register address.
+ * @param[out] data Register data.
+ * @return 0 if successful.
+ */
+int mpu_read_reg(unsigned char reg, unsigned char *data)
+{
+ if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
+ return -1;
+ if (reg >= st.hw->num_reg)
+ return -1;
+ return i2c_read(st.hw->addr, reg, 1, data);
+}
+
+/**
+ * @brief Initialize hardware.
+ * Initial configuration:\n
+ * Gyro FSR: +/- 2000DPS\n
+ * Accel FSR +/- 2G\n
+ * DLPF: 42Hz\n
+ * FIFO rate: 50Hz\n
+ * Clock source: Gyro PLL\n
+ * FIFO: Disabled.\n
+ * Data ready interrupt: Disabled, active low, unlatched.
+ * @param[in] int_param Platform-specific parameters to interrupt API.
+ * @return 0 if successful.
+ */
+int mpu_init(void)
+{
+ unsigned char data[6], rev;
+
+ /* Reset device. */
+ data[0] = BIT_RESET;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
+ return -1;
+ delay_ms(100);
+
+ /* Wake up chip. */
+ data[0] = 0x00;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
+ return -1;
+
+#if defined MPU6050
+ /* Check product revision. */
+ if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data))
+ return -1;
+ rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) |
+ (data[1] & 0x01);
+
+ if (rev) {
+ /* Congrats, these parts are better. */
+ if (rev == 1)
+ st.chip_cfg.accel_half = 1;
+ else if (rev == 2)
+ st.chip_cfg.accel_half = 0;
+ else {
+ //("Unsupported software product rev %d.\n", rev);
+ return -1;
+ }
+ } else {
+ if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data))
+ return -1;
+ rev = data[0] & 0x0F;
+ if (!rev) {
+ //("Product ID read as 0 indicates device is either "
+ //"incompatible or an MPU3050.\n");
+ return -1;
+ } else if (rev == 4) {
+ //("Half sensitivity part found.\n");
+ st.chip_cfg.accel_half = 1;
+ } else
+ st.chip_cfg.accel_half = 0;
+ }
+#elif defined MPU6500
+#define MPU6500_MEM_REV_ADDR (0x17)
+ if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
+ return -1;
+ if (rev == 0x1)
+ st.chip_cfg.accel_half = 0;
+ else {
+ //("Unsupported software product rev %d.\n", rev);
+ return -1;
+ }
+
+ /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
+ * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
+ */
+ data[0] = BIT_FIFO_SIZE_1024 | 0x8;
+ if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
+ return -1;
+#endif
+
+ /* Set to invalid values to ensure no I2C writes are skipped. */
+ st.chip_cfg.sensors = 0xFF;
+ st.chip_cfg.gyro_fsr = 0xFF;
+ st.chip_cfg.accel_fsr = 0xFF;
+ st.chip_cfg.lpf = 0xFF;
+ st.chip_cfg.sample_rate = 0xFFFF;
+ st.chip_cfg.fifo_enable = 0xFF;
+ st.chip_cfg.bypass_mode = 0xFF;
+#ifdef AK89xx_SECONDARY
+ st.chip_cfg.compass_sample_rate = 0xFFFF;
+#endif
+ /* mpu_set_sensors always preserves this setting. */
+ st.chip_cfg.clk_src = INV_CLK_PLL;
+ /* Handled in next call to mpu_set_bypass. */
+ st.chip_cfg.active_low_int = 1;
+ st.chip_cfg.latched_int = 0;
+ st.chip_cfg.int_motion_only = 0;
+ st.chip_cfg.lp_accel_mode = 0;
+ memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
+ st.chip_cfg.dmp_on = 0;
+ st.chip_cfg.dmp_loaded = 0;
+ st.chip_cfg.dmp_sample_rate = 0;
+
+ if (mpu_set_gyro_fsr(2000))
+ return -1;
+ if (mpu_set_accel_fsr(2))
+ return -1;
+ if (mpu_set_lpf(42))
+ return -1;
+ if (mpu_set_sample_rate(50))
+ return -1;
+ if (mpu_configure_fifo(0))
+ return -1;
+
+// if (int_param)
+// reg_int_cb(int_param);
+
+#ifdef AK89xx_SECONDARY
+ setup_compass();
+ if (mpu_set_compass_sample_rate(10))
+ return -1;
+#else
+ /* Already disabled by setup_compass. */
+ if (mpu_set_bypass(0))
+ return -1;
+#endif
+
+ mpu_set_sensors(0);
+ return 0;
+}
+
+/**
+ * @brief Enter low-power accel-only mode.
+ * In low-power accel mode, the chip goes to sleep and only wakes up to sample
+ * the accelerometer at one of the following frequencies:
+ * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
+ * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
+ * \n If the requested rate is not one listed above, the device will be set to
+ * the next highest rate. Requesting a rate above the maximum supported
+ * frequency will result in an error.
+ * \n To select a fractional wake-up frequency, round down the value passed to
+ * @e rate.
+ * @param[in] rate Minimum sampling rate, or zero to disable LP
+ * accel mode.
+ * @return 0 if successful.
+ */
+int mpu_lp_accel_mode(unsigned char rate)
+{
+ unsigned char tmp[2];
+
+ if (rate > 40)
+ return -1;
+
+ if (!rate) {
+ mpu_set_int_latched(0);
+ tmp[0] = 0;
+ tmp[1] = BIT_STBY_XYZG;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
+ return -1;
+ st.chip_cfg.lp_accel_mode = 0;
+ return 0;
+ }
+ /* For LP accel, we automatically configure the hardware to produce latched
+ * interrupts. In LP accel mode, the hardware cycles into sleep mode before
+ * it gets a chance to deassert the interrupt pin; therefore, we shift this
+ * responsibility over to the MCU.
+ *
+ * Any register read will clear the interrupt.
+ */
+ mpu_set_int_latched(1);
+#if defined MPU6050
+ tmp[0] = BIT_LPA_CYCLE;
+ if (rate == 1) {
+ tmp[1] = INV_LPA_1_25HZ;
+ mpu_set_lpf(5);
+ } else if (rate <= 5) {
+ tmp[1] = INV_LPA_5HZ;
+ mpu_set_lpf(5);
+ } else if (rate <= 20) {
+ tmp[1] = INV_LPA_20HZ;
+ mpu_set_lpf(10);
+ } else {
+ tmp[1] = INV_LPA_40HZ;
+ mpu_set_lpf(20);
+ }
+ tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
+ return -1;
+#elif defined MPU6500
+ /* Set wake frequency. */
+ if (rate == 1)
+ tmp[0] = INV_LPA_1_25HZ;
+ else if (rate == 2)
+ tmp[0] = INV_LPA_2_5HZ;
+ else if (rate <= 5)
+ tmp[0] = INV_LPA_5HZ;
+ else if (rate <= 10)
+ tmp[0] = INV_LPA_10HZ;
+ else if (rate <= 20)
+ tmp[0] = INV_LPA_20HZ;
+ else if (rate <= 40)
+ tmp[0] = INV_LPA_40HZ;
+ else if (rate <= 80)
+ tmp[0] = INV_LPA_80HZ;
+ else if (rate <= 160)
+ tmp[0] = INV_LPA_160HZ;
+ else if (rate <= 320)
+ tmp[0] = INV_LPA_320HZ;
+ else
+ tmp[0] = INV_LPA_640HZ;
+ if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
+ return -1;
+ tmp[0] = BIT_LPA_CYCLE;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
+ return -1;
+#endif
+ st.chip_cfg.sensors = INV_XYZ_ACCEL;
+ st.chip_cfg.clk_src = 0;
+ st.chip_cfg.lp_accel_mode = 1;
+ mpu_configure_fifo(0);
+
+ return 0;
+}
+
+/**
+ * @brief Read raw gyro data directly from the registers.
+ * @param[out] data Raw data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
+ * @return 0 if successful.
+ */
+int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
+{
+ unsigned char tmp[6];
+
+ if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
+ return -1;
+
+ if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
+ return -1;
+ data[0] = (tmp[0] << 8) | tmp[1];
+ data[1] = (tmp[2] << 8) | tmp[3];
+ data[2] = (tmp[4] << 8) | tmp[5];
+ if (timestamp)
+ get_ms(timestamp);
+ return 0;
+}
+
+/**
+ * @brief Read raw accel data directly from the registers.
+ * @param[out] data Raw data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
+ * @return 0 if successful.
+ */
+int mpu_get_accel_reg(short *data, unsigned long *timestamp)
+{
+ unsigned char tmp[6];
+
+ if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
+ return -1;
+
+ if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
+ return -1;
+ data[0] = (tmp[0] << 8) | tmp[1];
+ data[1] = (tmp[2] << 8) | tmp[3];
+ data[2] = (tmp[4] << 8) | tmp[5];
+ if (timestamp)
+ get_ms(timestamp);
+ return 0;
+}
+
+/**
+ * @brief Read temperature data directly from the registers.
+ * @param[out] data Data in q16 format.
+ * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
+ * @return 0 if successful.
+ */
+int mpu_get_temperature(long *data, unsigned long *timestamp)
+{
+ unsigned char tmp[2];
+ short raw;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
+ return -1;
+ raw = (tmp[0] << 8) | tmp[1];
+ if (timestamp)
+ get_ms(timestamp);
+
+ data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
+ return 0;
+}
+
+/**
+ * @brief Push biases to the accel bias registers.
+ * This function expects biases relative to the current sensor output, and
+ * these biases will be added to the factory-supplied values.
+ * @param[in] accel_bias New biases.
+ * @return 0 if successful.
+ */
+int mpu_set_accel_bias(const long *accel_bias)
+{
+ unsigned char data[6];
+ short accel_hw[3];
+ short got_accel[3];
+ short fg[3];
+
+ if (!accel_bias)
+ return -1;
+ if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
+ return 0;
+
+ if (i2c_read(st.hw->addr, 3, 3, data))
+ return -1;
+ fg[0] = ((data[0] >> 4) + 8) & 0xf;
+ fg[1] = ((data[1] >> 4) + 8) & 0xf;
+ fg[2] = ((data[2] >> 4) + 8) & 0xf;
+
+ accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
+ accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
+ accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
+
+ if (i2c_read(st.hw->addr, 0x06, 6, data))
+ return -1;
+
+ got_accel[0] = ((short)data[0] << 8) | data[1];
+ got_accel[1] = ((short)data[2] << 8) | data[3];
+ got_accel[2] = ((short)data[4] << 8) | data[5];
+
+ accel_hw[0] += got_accel[0];
+ accel_hw[1] += got_accel[1];
+ accel_hw[2] += got_accel[2];
+
+ data[0] = (accel_hw[0] >> 8) & 0xff;
+ data[1] = (accel_hw[0]) & 0xff;
+ data[2] = (accel_hw[1] >> 8) & 0xff;
+ data[3] = (accel_hw[1]) & 0xff;
+ data[4] = (accel_hw[2] >> 8) & 0xff;
+ data[5] = (accel_hw[2]) & 0xff;
+
+ if (i2c_write(st.hw->addr, 0x06, 6, data))
+ return -1;
+ return 0;
+}
+
+/**
+ * @brief Reset FIFO read/write pointers.
+ * @return 0 if successful.
+ */
+int mpu_reset_fifo(void)
+{
+ unsigned char data;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ data = 0;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
+ return -1;
+
+ if (st.chip_cfg.dmp_on) {
+ data = BIT_FIFO_RST | BIT_DMP_RST;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
+ return -1;
+ delay_ms(50);
+ data = BIT_DMP_EN | BIT_FIFO_EN;
+ if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
+ data |= BIT_AUX_IF_EN;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
+ return -1;
+ if (st.chip_cfg.int_enable)
+ data = BIT_DMP_INT_EN;
+ else
+ data = 0;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
+ return -1;
+ data = 0;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
+ return -1;
+ } else {
+ data = BIT_FIFO_RST;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
+ return -1;
+ if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
+ data = BIT_FIFO_EN;
+ else
+ data = BIT_FIFO_EN | BIT_AUX_IF_EN;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
+ return -1;
+ delay_ms(50);
+ if (st.chip_cfg.int_enable)
+ data = BIT_DATA_RDY_EN;
+ else
+ data = 0;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
+ return -1;
+ }
+ return 0;
+}
+
+/**
+ * @brief Get the gyro full-scale range.
+ * @param[out] fsr Current full-scale range.
+ * @return 0 if successful.
+ */
+int mpu_get_gyro_fsr(unsigned short *fsr)
+{
+ switch (st.chip_cfg.gyro_fsr) {
+ case INV_FSR_250DPS:
+ fsr[0] = 250;
+ break;
+ case INV_FSR_500DPS:
+ fsr[0] = 500;
+ break;
+ case INV_FSR_1000DPS:
+ fsr[0] = 1000;
+ break;
+ case INV_FSR_2000DPS:
+ fsr[0] = 2000;
+ break;
+ default:
+ fsr[0] = 0;
+ break;
+ }
+ return 0;
+}
+
+/**
+ * @brief Set the gyro full-scale range.
+ * @param[in] fsr Desired full-scale range.
+ * @return 0 if successful.
+ */
+int mpu_set_gyro_fsr(unsigned short fsr)
+{
+ unsigned char data;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ switch (fsr) {
+ case 250:
+ data = INV_FSR_250DPS << 3;
+ break;
+ case 500:
+ data = INV_FSR_500DPS << 3;
+ break;
+ case 1000:
+ data = INV_FSR_1000DPS << 3;
+ break;
+ case 2000:
+ data = INV_FSR_2000DPS << 3;
+ break;
+ default:
+ return -1;
+ }
+
+ if (st.chip_cfg.gyro_fsr == (data >> 3))
+ return 0;
+ if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
+ return -1;
+ st.chip_cfg.gyro_fsr = data >> 3;
+ return 0;
+}
+
+/**
+ * @brief Get the accel full-scale range.
+ * @param[out] fsr Current full-scale range.
+ * @return 0 if successful.
+ */
+int mpu_get_accel_fsr(unsigned char *fsr)
+{
+ switch (st.chip_cfg.accel_fsr) {
+ case INV_FSR_2G:
+ fsr[0] = 2;
+ break;
+ case INV_FSR_4G:
+ fsr[0] = 4;
+ break;
+ case INV_FSR_8G:
+ fsr[0] = 8;
+ break;
+ case INV_FSR_16G:
+ fsr[0] = 16;
+ break;
+ default:
+ return -1;
+ }
+ if (st.chip_cfg.accel_half)
+ fsr[0] <<= 1;
+ return 0;
+}
+
+/**
+ * @brief Set the accel full-scale range.
+ * @param[in] fsr Desired full-scale range.
+ * @return 0 if successful.
+ */
+int mpu_set_accel_fsr(unsigned char fsr)
+{
+ unsigned char data;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ switch (fsr) {
+ case 2:
+ data = INV_FSR_2G << 3;
+ break;
+ case 4:
+ data = INV_FSR_4G << 3;
+ break;
+ case 8:
+ data = INV_FSR_8G << 3;
+ break;
+ case 16:
+ data = INV_FSR_16G << 3;
+ break;
+ default:
+ return -1;
+ }
+
+ if (st.chip_cfg.accel_fsr == (data >> 3))
+ return 0;
+ if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
+ return -1;
+ st.chip_cfg.accel_fsr = data >> 3;
+ return 0;
+}
+
+/**
+ * @brief Get the current DLPF setting.
+ * @param[out] lpf Current LPF setting.
+ * 0 if successful.
+ */
+int mpu_get_lpf(unsigned short *lpf)
+{
+ switch (st.chip_cfg.lpf) {
+ case INV_FILTER_188HZ:
+ lpf[0] = 188;
+ break;
+ case INV_FILTER_98HZ:
+ lpf[0] = 98;
+ break;
+ case INV_FILTER_42HZ:
+ lpf[0] = 42;
+ break;
+ case INV_FILTER_20HZ:
+ lpf[0] = 20;
+ break;
+ case INV_FILTER_10HZ:
+ lpf[0] = 10;
+ break;
+ case INV_FILTER_5HZ:
+ lpf[0] = 5;
+ break;
+ case INV_FILTER_256HZ_NOLPF2:
+ case INV_FILTER_2100HZ_NOLPF:
+ default:
+ lpf[0] = 0;
+ break;
+ }
+ return 0;
+}
+
+/**
+ * @brief Set digital low pass filter.
+ * The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
+ * @param[in] lpf Desired LPF setting.
+ * @return 0 if successful.
+ */
+int mpu_set_lpf(unsigned short lpf)
+{
+ unsigned char data;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ if (lpf >= 188)
+ data = INV_FILTER_188HZ;
+ else if (lpf >= 98)
+ data = INV_FILTER_98HZ;
+ else if (lpf >= 42)
+ data = INV_FILTER_42HZ;
+ else if (lpf >= 20)
+ data = INV_FILTER_20HZ;
+ else if (lpf >= 10)
+ data = INV_FILTER_10HZ;
+ else
+ data = INV_FILTER_5HZ;
+
+ if (st.chip_cfg.lpf == data)
+ return 0;
+ if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
+ return -1;
+ st.chip_cfg.lpf = data;
+ return 0;
+}
+
+/**
+ * @brief Get sampling rate.
+ * @param[out] rate Current sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int mpu_get_sample_rate(unsigned short *rate)
+{
+ if (st.chip_cfg.dmp_on)
+ return -1;
+ else
+ rate[0] = st.chip_cfg.sample_rate;
+ return 0;
+}
+
+/**
+ * @brief Set sampling rate.
+ * Sampling rate must be between 4Hz and 1kHz.
+ * @param[in] rate Desired sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int mpu_set_sample_rate(unsigned short rate)
+{
+ unsigned char data;
+
+ if (!(st.chip_cfg.sensors))
+ return -1;
+
+ if (st.chip_cfg.dmp_on)
+ return -1;
+ else {
+ if (st.chip_cfg.lp_accel_mode) {
+ if (rate && (rate <= 40)) {
+ /* Just stay in low-power accel mode. */
+ mpu_lp_accel_mode(rate);
+ return 0;
+ }
+ /* Requested rate exceeds the allowed frequencies in LP accel mode,
+ * switch back to full-power mode.
+ */
+ mpu_lp_accel_mode(0);
+ }
+ if (rate < 4)
+ rate = 4;
+ else if (rate > 1000)
+ rate = 1000;
+
+ data = 1000 / rate - 1;
+ if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))
+ return -1;
+
+ st.chip_cfg.sample_rate = 1000 / (1 + data);
+
+#ifdef AK89xx_SECONDARY
+ mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
+#endif
+
+ /* Automatically set LPF to 1/2 sampling rate. */
+ mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
+ return 0;
+ }
+}
+
+/**
+ * @brief Get compass sampling rate.
+ * @param[out] rate Current compass sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int mpu_get_compass_sample_rate(unsigned short *rate)
+{
+#ifdef AK89xx_SECONDARY
+ rate[0] = st.chip_cfg.compass_sample_rate;
+ return 0;
+#else
+ rate[0] = 0;
+ return -1;
+#endif
+}
+
+/**
+ * @brief Set compass sampling rate.
+ * The compass on the auxiliary I2C bus is read by the MPU hardware at a
+ * maximum of 100Hz. The actual rate can be set to a fraction of the gyro
+ * sampling rate.
+ *
+ * \n WARNING: The new rate may be different than what was requested. Call
+ * mpu_get_compass_sample_rate to check the actual setting.
+ * @param[in] rate Desired compass sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int mpu_set_compass_sample_rate(unsigned short rate)
+{
+#ifdef AK89xx_SECONDARY
+ unsigned char div;
+ if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
+ return -1;
+
+ div = st.chip_cfg.sample_rate / rate - 1;
+ if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
+ return -1;
+ st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
+ return 0;
+#else
+ return -1;
+#endif
+}
+
+/**
+ * @brief Get gyro sensitivity scale factor.
+ * @param[out] sens Conversion from hardware units to dps.
+ * @return 0 if successful.
+ */
+int mpu_get_gyro_sens(float *sens)
+{
+ switch (st.chip_cfg.gyro_fsr) {
+ case INV_FSR_250DPS:
+ sens[0] = 131.f;
+ break;
+ case INV_FSR_500DPS:
+ sens[0] = 65.5f;
+ break;
+ case INV_FSR_1000DPS:
+ sens[0] = 32.8f;
+ break;
+ case INV_FSR_2000DPS:
+ sens[0] = 16.4f;
+ break;
+ default:
+ return -1;
+ }
+ return 0;
+}
+
+/**
+ * @brief Get accel sensitivity scale factor.
+ * @param[out] sens Conversion from hardware units to g's.
+ * @return 0 if successful.
+ */
+int mpu_get_accel_sens(unsigned short *sens)
+{
+ switch (st.chip_cfg.accel_fsr) {
+ case INV_FSR_2G:
+ sens[0] = 16384;
+ break;
+ case INV_FSR_4G:
+ sens[0] = 8092;
+ break;
+ case INV_FSR_8G:
+ sens[0] = 4096;
+ break;
+ case INV_FSR_16G:
+ sens[0] = 2048;
+ break;
+ default:
+ return -1;
+ }
+ if (st.chip_cfg.accel_half)
+ sens[0] >>= 1;
+ return 0;
+}
+
+/**
+ * @brief Get current FIFO configuration.
+ * @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * @param[out] sensors Mask of sensors in FIFO.
+ * @return 0 if successful.
+ */
+int mpu_get_fifo_config(unsigned char *sensors)
+{
+ sensors[0] = st.chip_cfg.fifo_enable;
+ return 0;
+}
+
+/**
+ * @brief Select which sensors are pushed to FIFO.
+ * @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * @param[in] sensors Mask of sensors to push to FIFO.
+ * @return 0 if successful.
+ */
+int mpu_configure_fifo(unsigned char sensors)
+{
+ unsigned char prev;
+ int result = 0;
+
+ /* Compass data isn't going into the FIFO. Stop trying. */
+ sensors &= ~INV_XYZ_COMPASS;
+
+ if (st.chip_cfg.dmp_on)
+ return 0;
+ else {
+ if (!(st.chip_cfg.sensors))
+ return -1;
+ prev = st.chip_cfg.fifo_enable;
+ st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
+ if (st.chip_cfg.fifo_enable != sensors)
+ /* You're not getting what you asked for. Some sensors are
+ * asleep.
+ */
+ result = -1;
+ else
+ result = 0;
+ if (sensors || st.chip_cfg.lp_accel_mode)
+ set_int_enable(1);
+ else
+ set_int_enable(0);
+ if (sensors) {
+ if (mpu_reset_fifo()) {
+ st.chip_cfg.fifo_enable = prev;
+ return -1;
+ }
+ }
+ }
+
+ return result;
+}
+
+/**
+ * @brief Get current power state.
+ * @param[in] power_on 1 if turned on, 0 if suspended.
+ * @return 0 if successful.
+ */
+int mpu_get_power_state(unsigned char *power_on)
+{
+ if (st.chip_cfg.sensors)
+ power_on[0] = 1;
+ else
+ power_on[0] = 0;
+ return 0;
+}
+
+/**
+ * @brief Turn specific sensors on/off.
+ * @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * \n INV_XYZ_COMPASS
+ * @param[in] sensors Mask of sensors to wake.
+ * @return 0 if successful.
+ */
+int mpu_set_sensors(unsigned char sensors)
+{
+ unsigned char data;
+#ifdef AK89xx_SECONDARY
+ unsigned char user_ctrl;
+#endif
+
+ if (sensors & INV_XYZ_GYRO)
+ data = INV_CLK_PLL;
+ else if (sensors)
+ data = 0;
+ else
+ data = BIT_SLEEP;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
+ st.chip_cfg.sensors = 0;
+ return -1;
+ }
+ st.chip_cfg.clk_src = data & ~BIT_SLEEP;
+
+ data = 0;
+ if (!(sensors & INV_X_GYRO))
+ data |= BIT_STBY_XG;
+ if (!(sensors & INV_Y_GYRO))
+ data |= BIT_STBY_YG;
+ if (!(sensors & INV_Z_GYRO))
+ data |= BIT_STBY_ZG;
+ if (!(sensors & INV_XYZ_ACCEL))
+ data |= BIT_STBY_XYZA;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
+ st.chip_cfg.sensors = 0;
+ return -1;
+ }
+
+ if (sensors && (sensors != INV_XYZ_ACCEL))
+ /* Latched interrupts only used in LP accel mode. */
+ mpu_set_int_latched(0);
+
+#ifdef AK89xx_SECONDARY
+#ifdef AK89xx_BYPASS
+ if (sensors & INV_XYZ_COMPASS)
+ mpu_set_bypass(1);
+ else
+ mpu_set_bypass(0);
+#else
+ if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
+ return -1;
+ /* Handle AKM power management. */
+ if (sensors & INV_XYZ_COMPASS) {
+ data = AKM_SINGLE_MEASUREMENT;
+ user_ctrl |= BIT_AUX_IF_EN;
+ } else {
+ data = AKM_POWER_DOWN;
+ user_ctrl &= ~BIT_AUX_IF_EN;
+ }
+ if (st.chip_cfg.dmp_on)
+ user_ctrl |= BIT_DMP_EN;
+ else
+ user_ctrl &= ~BIT_DMP_EN;
+ if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
+ return -1;
+ /* Enable/disable I2C master mode. */
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
+ return -1;
+#endif
+#endif
+
+ st.chip_cfg.sensors = sensors;
+ st.chip_cfg.lp_accel_mode = 0;
+ delay_ms(50);
+ return 0;
+}
+
+/**
+ * @brief Read the MPU interrupt status registers.
+ * @param[out] status Mask of interrupt bits.
+ * @return 0 if successful.
+ */
+int mpu_get_int_status(short *status)
+{
+ unsigned char tmp[2];
+ if (!st.chip_cfg.sensors)
+ return -1;
+ if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
+ return -1;
+ status[0] = (tmp[0] << 8) | tmp[1];
+ return 0;
+}
+
+/**
+ * @brief Get one packet from the FIFO.
+ * If @e sensors does not contain a particular sensor, disregard the data
+ * returned to that pointer.
+ * \n @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * \n If the FIFO has no new data, @e sensors will be zero.
+ * \n If the FIFO is disabled, @e sensors will be zero and this function will
+ * return a non-zero error code.
+ * @param[out] gyro Gyro data in hardware units.
+ * @param[out] accel Accel data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds.
+ * @param[out] sensors Mask of sensors read from FIFO.
+ * @param[out] more Number of remaining packets.
+ * @return 0 if successful.
+ */
+int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
+ unsigned char *sensors, unsigned char *more)
+{
+ /* Assumes maximum packet size is gyro (6) + accel (6). */
+ unsigned char data[MAX_PACKET_LENGTH];
+ unsigned char packet_size = 0;
+ unsigned short fifo_count, index = 0;
+
+ if (st.chip_cfg.dmp_on)
+ return -1;
+
+ sensors[0] = 0;
+ if (!st.chip_cfg.sensors)
+ return -1;
+ if (!st.chip_cfg.fifo_enable)
+ return -1;
+
+ if (st.chip_cfg.fifo_enable & INV_X_GYRO)
+ packet_size += 2;
+ if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
+ packet_size += 2;
+ if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
+ packet_size += 2;
+ if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
+ packet_size += 6;
+
+ if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
+ return -1;
+ fifo_count = (data[0] << 8) | data[1];
+ if (fifo_count < packet_size)
+ return 0;
+// //("FIFO count: %hd\n", fifo_count);
+ if (fifo_count > (st.hw->max_fifo >> 1)) {
+ /* FIFO is 50% full, better check overflow bit. */
+ if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
+ return -1;
+ if (data[0] & BIT_FIFO_OVERFLOW) {
+ mpu_reset_fifo();
+ return -2;
+ }
+ }
+ get_ms((unsigned long*)timestamp);
+
+ if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
+ return -1;
+ more[0] = fifo_count / packet_size - 1;
+ sensors[0] = 0;
+
+ if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
+ accel[0] = (data[index+0] << 8) | data[index+1];
+ accel[1] = (data[index+2] << 8) | data[index+3];
+ accel[2] = (data[index+4] << 8) | data[index+5];
+ sensors[0] |= INV_XYZ_ACCEL;
+ index += 6;
+ }
+ if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
+ gyro[0] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_X_GYRO;
+ index += 2;
+ }
+ if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
+ gyro[1] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_Y_GYRO;
+ index += 2;
+ }
+ if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
+ gyro[2] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_Z_GYRO;
+ index += 2;
+ }
+
+ return 0;
+}
+
+/**
+ * @brief Get one unparsed packet from the FIFO.
+ * This function should be used if the packet is to be parsed elsewhere.
+ * @param[in] length Length of one FIFO packet.
+ * @param[in] data FIFO packet.
+ * @param[in] more Number of remaining packets.
+ */
+int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
+ unsigned char *more)
+{
+ unsigned char tmp[2];
+ unsigned short fifo_count;
+ if (!st.chip_cfg.dmp_on)
+ return -1;
+ if (!st.chip_cfg.sensors)
+ return -1;
+
+ if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
+ return -1;
+ fifo_count = (tmp[0] << 8) | tmp[1];
+ if (fifo_count < length) {
+ more[0] = 0;
+ return -1;
+ }
+ if (fifo_count > (st.hw->max_fifo >> 1)) {
+ /* FIFO is 50% full, better check overflow bit. */
+ if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
+ return -1;
+ if (tmp[0] & BIT_FIFO_OVERFLOW) {
+ mpu_reset_fifo();
+ return -2;
+ }
+ }
+
+ if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
+ return -1;
+ more[0] = fifo_count / length - 1;
+ return 0;
+}
+
+/**
+ * @brief Set device to bypass mode.
+ * @param[in] bypass_on 1 to enable bypass mode.
+ * @return 0 if successful.
+ */
+int mpu_set_bypass(unsigned char bypass_on)
+{
+ unsigned char tmp;
+
+ if (st.chip_cfg.bypass_mode == bypass_on)
+ return 0;
+
+ if (bypass_on) {
+ if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
+ return -1;
+ tmp &= ~BIT_AUX_IF_EN;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
+ return -1;
+ delay_ms(3);
+ tmp = BIT_BYPASS_EN;
+ if (st.chip_cfg.active_low_int)
+ tmp |= BIT_ACTL;
+ if (st.chip_cfg.latched_int)
+ tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
+ if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
+ return -1;
+ } else {
+ /* Enable I2C master mode if compass is being used. */
+ if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
+ return -1;
+ if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
+ tmp |= BIT_AUX_IF_EN;
+ else
+ tmp &= ~BIT_AUX_IF_EN;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
+ return -1;
+ delay_ms(3);
+ if (st.chip_cfg.active_low_int)
+ tmp = BIT_ACTL;
+ else
+ tmp = 0;
+ if (st.chip_cfg.latched_int)
+ tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
+ if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
+ return -1;
+ }
+ st.chip_cfg.bypass_mode = bypass_on;
+ return 0;
+}
+
+/**
+ * @brief Set interrupt level.
+ * @param[in] active_low 1 for active low, 0 for active high.
+ * @return 0 if successful.
+ */
+int mpu_set_int_level(unsigned char active_low)
+{
+ st.chip_cfg.active_low_int = active_low;
+ return 0;
+}
+
+/**
+ * @brief Enable latched interrupts.
+ * Any MPU register will clear the interrupt.
+ * @param[in] enable 1 to enable, 0 to disable.
+ * @return 0 if successful.
+ */
+int mpu_set_int_latched(unsigned char enable)
+{
+ unsigned char tmp;
+ if (st.chip_cfg.latched_int == enable)
+ return 0;
+
+ if (enable)
+ tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
+ else
+ tmp = 0;
+ if (st.chip_cfg.bypass_mode)
+ tmp |= BIT_BYPASS_EN;
+ if (st.chip_cfg.active_low_int)
+ tmp |= BIT_ACTL;
+ if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
+ return -1;
+ st.chip_cfg.latched_int = enable;
+ return 0;
+}
+
+#ifdef MPU6050
+static int get_accel_prod_shift(float *st_shift)
+{
+ unsigned char tmp[4], shift_code[3], ii;
+
+ if (i2c_read(st.hw->addr, 0x0D, 4, tmp))
+ return 0x07;
+
+ shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
+ shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
+ shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
+ for (ii = 0; ii < 3; ii++) {
+ if (!shift_code[ii]) {
+ st_shift[ii] = 0.f;
+ continue;
+ }
+ /* Equivalent to..
+ * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
+ */
+ st_shift[ii] = 0.34f;
+ while (--shift_code[ii])
+ st_shift[ii] *= 1.034f;
+ }
+ return 0;
+}
+
+static int accel_self_test(long *bias_regular, long *bias_st)
+{
+ int jj, result = 0;
+ float st_shift[3], st_shift_cust, st_shift_var;
+
+ get_accel_prod_shift(st_shift);
+ for(jj = 0; jj < 3; jj++) {
+ st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
+ if (st_shift[jj]) {
+ st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
+ if (fabs(st_shift_var) > test.max_accel_var)
+ result |= 1 << jj;
+ } else if ((st_shift_cust < test.min_g) ||
+ (st_shift_cust > test.max_g))
+ result |= 1 << jj;
+ }
+
+ return result;
+}
+
+static int gyro_self_test(long *bias_regular, long *bias_st)
+{
+ int jj, result = 0;
+ unsigned char tmp[3];
+ float st_shift, st_shift_cust, st_shift_var;
+
+ if (i2c_read(st.hw->addr, 0x0D, 3, tmp))
+ return 0x07;
+
+ tmp[0] &= 0x1F;
+ tmp[1] &= 0x1F;
+ tmp[2] &= 0x1F;
+
+ for (jj = 0; jj < 3; jj++) {
+ st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
+ if (tmp[jj]) {
+ st_shift = 3275.f / test.gyro_sens;
+ while (--tmp[jj])
+ st_shift *= 1.046f;
+ st_shift_var = st_shift_cust / st_shift - 1.f;
+ if (fabs(st_shift_var) > test.max_gyro_var)
+ result |= 1 << jj;
+ } else if ((st_shift_cust < test.min_dps) ||
+ (st_shift_cust > test.max_dps))
+ result |= 1 << jj;
+ }
+ return result;
+}
+
+#ifdef AK89xx_SECONDARY
+static int compass_self_test(void)
+{
+ unsigned char tmp[6];
+ unsigned char tries = 10;
+ int result = 0x07;
+ short data;
+
+ mpu_set_bypass(1);
+
+ tmp[0] = AKM_POWER_DOWN;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
+ return 0x07;
+ tmp[0] = AKM_BIT_SELF_TEST;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
+ goto AKM_restore;
+ tmp[0] = AKM_MODE_SELF_TEST;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
+ goto AKM_restore;
+
+ do {
+ delay_ms(10);
+ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
+ goto AKM_restore;
+ if (tmp[0] & AKM_DATA_READY)
+ break;
+ } while (tries--);
+ if (!(tmp[0] & AKM_DATA_READY))
+ goto AKM_restore;
+
+ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
+ goto AKM_restore;
+
+ result = 0;
+ data = (short)(tmp[1] << 8) | tmp[0];
+ if ((data > 100) || (data < -100))
+ result |= 0x01;
+ data = (short)(tmp[3] << 8) | tmp[2];
+ if ((data > 100) || (data < -100))
+ result |= 0x02;
+ data = (short)(tmp[5] << 8) | tmp[4];
+ if ((data > -300) || (data < -1000))
+ result |= 0x04;
+
+AKM_restore:
+ tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
+ i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
+ tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
+ i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
+ mpu_set_bypass(0);
+ return result;
+}
+#endif
+#endif
+
+static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
+{
+ unsigned char data[MAX_PACKET_LENGTH];
+ unsigned char packet_count, ii;
+ unsigned short fifo_count;
+
+ data[0] = 0x01;
+ data[1] = 0;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
+ return -1;
+ delay_ms(200);
+ data[0] = 0;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
+ return -1;
+ data[0] = BIT_FIFO_RST | BIT_DMP_RST;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
+ return -1;
+ delay_ms(15);
+ data[0] = st.test->reg_lpf;
+ if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
+ return -1;
+ data[0] = st.test->reg_rate_div;
+ if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
+ return -1;
+ if (hw_test)
+ data[0] = st.test->reg_gyro_fsr | 0xE0;
+ else
+ data[0] = st.test->reg_gyro_fsr;
+ if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
+ return -1;
+
+ if (hw_test)
+ data[0] = st.test->reg_accel_fsr | 0xE0;
+ else
+ data[0] = test.reg_accel_fsr;
+ if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
+ return -1;
+ if (hw_test)
+ delay_ms(200);
+
+ /* Fill FIFO for test.wait_ms milliseconds. */
+ data[0] = BIT_FIFO_EN;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
+ return -1;
+
+ data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
+ return -1;
+ delay_ms(test.wait_ms);
+ data[0] = 0;
+ if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
+ return -1;
+
+ if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
+ return -1;
+
+ fifo_count = (data[0] << 8) | data[1];
+ packet_count = fifo_count / MAX_PACKET_LENGTH;
+ gyro[0] = gyro[1] = gyro[2] = 0;
+ accel[0] = accel[1] = accel[2] = 0;
+
+ for (ii = 0; ii < packet_count; ii++) {
+ short accel_cur[3], gyro_cur[3];
+ if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
+ return -1;
+ accel_cur[0] = ((short)data[0] << 8) | data[1];
+ accel_cur[1] = ((short)data[2] << 8) | data[3];
+ accel_cur[2] = ((short)data[4] << 8) | data[5];
+ accel[0] += (long)accel_cur[0];
+ accel[1] += (long)accel_cur[1];
+ accel[2] += (long)accel_cur[2];
+ gyro_cur[0] = (((short)data[6] << 8) | data[7]);
+ gyro_cur[1] = (((short)data[8] << 8) | data[9]);
+ gyro_cur[2] = (((short)data[10] << 8) | data[11]);
+ gyro[0] += (long)gyro_cur[0];
+ gyro[1] += (long)gyro_cur[1];
+ gyro[2] += (long)gyro_cur[2];
+ }
+#ifdef EMPL_NO_64BIT
+ gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
+ gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
+ gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
+ if (has_accel) {
+ accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
+ packet_count);
+ accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
+ packet_count);
+ accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
+ packet_count);
+ /* Don't remove gravity! */
+ accel[2] -= 65536L;
+ }
+#else
+ gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
+ gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
+ gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
+ accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
+ packet_count);
+ accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
+ packet_count);
+ accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
+ packet_count);
+ /* Don't remove gravity! */
+ if (accel[2] > 0L)
+ accel[2] -= 65536L;
+ else
+ accel[2] += 65536L;
+#endif
+
+ return 0;
+}
+
+/**
+ * @brief Trigger gyro/accel/compass self-test.
+ * On success/error, the self-test returns a mask representing the sensor(s)
+ * that failed. For each bit, a one (1) represents a "pass" case; conversely,
+ * a zero (0) indicates a failure.
+ *
+ * \n The mask is defined as follows:
+ * \n Bit 0: Gyro.
+ * \n Bit 1: Accel.
+ * \n Bit 2: Compass.
+ *
+ * \n Currently, the hardware self-test is unsupported for MPU6500. However,
+ * this function can still be used to obtain the accel and gyro biases.
+ *
+ * \n This function must be called with the device either face-up or face-down
+ * (z-axis is parallel to gravity).
+ * @param[out] gyro Gyro biases in q16 format.
+ * @param[out] accel Accel biases (if applicable) in q16 format.
+ * @return Result mask (see above).
+ */
+int mpu_run_self_test(long *gyro, long *accel)
+{
+#ifdef MPU6050
+ const unsigned char tries = 2;
+ long gyro_st[3], accel_st[3];
+ unsigned char accel_result, gyro_result;
+#ifdef AK89xx_SECONDARY
+ unsigned char compass_result;
+#endif
+ int ii;
+#endif
+ int result;
+ unsigned char accel_fsr, fifo_sensors, sensors_on;
+ unsigned short gyro_fsr, sample_rate, lpf;
+ unsigned char dmp_was_on;
+
+ if (st.chip_cfg.dmp_on) {
+ mpu_set_dmp_state(0);
+ dmp_was_on = 1;
+ } else
+ dmp_was_on = 0;
+
+ /* Get initial settings. */
+ mpu_get_gyro_fsr(&gyro_fsr);
+ mpu_get_accel_fsr(&accel_fsr);
+ mpu_get_lpf(&lpf);
+ mpu_get_sample_rate(&sample_rate);
+ sensors_on = st.chip_cfg.sensors;
+ mpu_get_fifo_config(&fifo_sensors);
+
+ /* For older chips, the self-test will be different. */
+#if defined MPU6050
+ for (ii = 0; ii < tries; ii++)
+ if (!get_st_biases(gyro, accel, 0))
+ break;
+ if (ii == tries) {
+ /* If we reach this point, we most likely encountered an I2C error.
+ * We'll just report an error for all three sensors.
+ */
+ result = 0;
+ goto restore;
+ }
+ for (ii = 0; ii < tries; ii++)
+ if (!get_st_biases(gyro_st, accel_st, 1))
+ break;
+ if (ii == tries) {
+ /* Again, probably an I2C error. */
+ result = 0;
+ goto restore;
+ }
+ accel_result = accel_self_test(accel, accel_st);
+ gyro_result = gyro_self_test(gyro, gyro_st);
+
+ result = 0;
+ if (!gyro_result)
+ result |= 0x01;
+ if (!accel_result)
+ result |= 0x02;
+
+#ifdef AK89xx_SECONDARY
+ compass_result = compass_self_test();
+ if (!compass_result)
+ result |= 0x04;
+#endif
+restore:
+#elif defined MPU6500
+ /* For now, this function will return a "pass" result for all three sensors
+ * for compatibility with current test applications.
+ */
+ get_st_biases(gyro, accel, 0);
+ result = 0x7;
+#endif
+ /* Set to invalid values to ensure no I2C writes are skipped. */
+ st.chip_cfg.gyro_fsr = 0xFF;
+ st.chip_cfg.accel_fsr = 0xFF;
+ st.chip_cfg.lpf = 0xFF;
+ st.chip_cfg.sample_rate = 0xFFFF;
+ st.chip_cfg.sensors = 0xFF;
+ st.chip_cfg.fifo_enable = 0xFF;
+ st.chip_cfg.clk_src = INV_CLK_PLL;
+ mpu_set_gyro_fsr(gyro_fsr);
+ mpu_set_accel_fsr(accel_fsr);
+ mpu_set_lpf(lpf);
+ mpu_set_sample_rate(sample_rate);
+ mpu_set_sensors(sensors_on);
+ mpu_configure_fifo(fifo_sensors);
+
+ if (dmp_was_on)
+ mpu_set_dmp_state(1);
+
+ return result;
+}
+
+/**
+ * @brief Write to the DMP memory.
+ * This function prevents I2C writes past the bank boundaries. The DMP memory
+ * is only accessible when the chip is awake.
+ * @param[in] mem_addr Memory location (bank << 8 | start address)
+ * @param[in] length Number of bytes to write.
+ * @param[in] data Bytes to write to memory.
+ * @return 0 if successful.
+ */
+int mpu_write_mem(unsigned short mem_addr, unsigned short length,
+ unsigned char *data)
+{
+ unsigned char tmp[2];
+
+ if (!data)
+ return -1;
+ if (!st.chip_cfg.sensors)
+ return -1;
+
+ tmp[0] = (unsigned char)(mem_addr >> 8);
+ tmp[1] = (unsigned char)(mem_addr & 0xFF);
+
+ /* Check bank boundaries. */
+ if (tmp[1] + length > st.hw->bank_size)
+ return -1;
+
+ if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
+ return -1;
+ if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
+ return -1;
+ return 0;
+}
+
+/**
+ * @brief Read from the DMP memory.
+ * This function prevents I2C reads past the bank boundaries. The DMP memory
+ * is only accessible when the chip is awake.
+ * @param[in] mem_addr Memory location (bank << 8 | start address)
+ * @param[in] length Number of bytes to read.
+ * @param[out] data Bytes read from memory.
+ * @return 0 if successful.
+ */
+int mpu_read_mem(unsigned short mem_addr, unsigned short length,
+ unsigned char *data)
+{
+ unsigned char tmp[2];
+
+ if (!data)
+ return -1;
+ if (!st.chip_cfg.sensors)
+ return -1;
+
+ tmp[0] = (unsigned char)(mem_addr >> 8);
+ tmp[1] = (unsigned char)(mem_addr & 0xFF);
+
+ /* Check bank boundaries. */
+ if (tmp[1] + length > st.hw->bank_size)
+ return -1;
+
+ if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
+ return -1;
+ if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
+ return -1;
+ return 0;
+}
+
+/**
+ * @brief Load and verify DMP image.
+ * @param[in] length Length of DMP image.
+ * @param[in] firmware DMP code.
+ * @param[in] start_addr Starting address of DMP code memory.
+ * @param[in] sample_rate Fixed sampling rate used when DMP is enabled.
+ * @return 0 if successful.
+ */
+int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
+ unsigned short start_addr, unsigned short sample_rate)
+{
+ unsigned short ii;
+ unsigned short this_write;
+ /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
+#define LOAD_CHUNK (16)
+ unsigned char cur[LOAD_CHUNK], tmp[2];
+
+ if (st.chip_cfg.dmp_loaded)
+ /* DMP should only be loaded once. */
+ return -1;
+
+ if (!firmware)
+ return -1;
+ for (ii = 0; ii < length; ii += this_write) {
+ this_write = min(LOAD_CHUNK, length - ii);
+ if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
+ return -1;
+ if (mpu_read_mem(ii, this_write, cur))
+ return -1;
+ if (memcmp(firmware+ii, cur, this_write))
+ return -2;
+ }
+
+ /* Set program start address. */
+ tmp[0] = start_addr >> 8;
+ tmp[1] = start_addr & 0xFF;
+ if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
+ return -1;
+
+ st.chip_cfg.dmp_loaded = 1;
+ st.chip_cfg.dmp_sample_rate = sample_rate;
+ return 0;
+}
+
+/**
+ * @brief Enable/disable DMP support.
+ * @param[in] enable 1 to turn on the DMP.
+ * @return 0 if successful.
+ */
+int mpu_set_dmp_state(unsigned char enable)
+{
+ unsigned char tmp;
+ if (st.chip_cfg.dmp_on == enable)
+ return 0;
+
+ if (enable) {
+ if (!st.chip_cfg.dmp_loaded)
+ return -1;
+ /* Disable data ready interrupt. */
+ set_int_enable(0);
+ /* Disable bypass mode. */
+ mpu_set_bypass(0);
+ /* Keep constant sample rate, FIFO rate controlled by DMP. */
+ mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
+ /* Remove FIFO elements. */
+ tmp = 0;
+ i2c_write(st.hw->addr, 0x23, 1, &tmp);
+ st.chip_cfg.dmp_on = 1;
+ /* Enable DMP interrupt. */
+ set_int_enable(1);
+ mpu_reset_fifo();
+ } else {
+ /* Disable DMP interrupt. */
+ set_int_enable(0);
+ /* Restore FIFO settings. */
+ tmp = st.chip_cfg.fifo_enable;
+ i2c_write(st.hw->addr, 0x23, 1, &tmp);
+ st.chip_cfg.dmp_on = 0;
+ mpu_reset_fifo();
+ }
+ return 0;
+}
+
+/**
+ * @brief Get DMP state.
+ * @param[out] enabled 1 if enabled.
+ * @return 0 if successful.
+ */
+int mpu_get_dmp_state(unsigned char *enabled)
+{
+ enabled[0] = st.chip_cfg.dmp_on;
+ return 0;
+}
+
+
+/* This initialization is similar to the one in ak8975.c. */
+int setup_compass(void)
+{
+#ifdef AK89xx_SECONDARY
+ unsigned char data[4], akm_addr;
+
+ mpu_set_bypass(1);
+
+ /* Find compass. Possible addresses range from 0x0C to 0x0F. */
+ for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
+ int result;
+ result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
+ if (!result && (data[0] == AKM_WHOAMI))
+ break;
+ }
+
+ if (akm_addr > 0x0F) {
+ /* TODO: Handle this case in all compass-related functions. */
+ //("Compass not found.\n");
+ return -1;
+ }
+
+ st.chip_cfg.compass_addr = akm_addr;
+
+ data[0] = AKM_POWER_DOWN;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ data[0] = AKM_FUSE_ROM_ACCESS;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ /* Get sensitivity adjustment data from fuse ROM. */
+ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
+ return -1;
+ st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
+ st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
+ st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
+
+ data[0] = AKM_POWER_DOWN;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ mpu_set_bypass(0);
+
+ /* Set up master mode, master clock, and ES bit. */
+ data[0] = 0x40;
+ if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
+ return -1;
+
+ /* Slave 0 reads from AKM data registers. */
+ data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
+ if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
+ return -1;
+
+ /* Compass reads start at this register. */
+ data[0] = AKM_REG_ST1;
+ if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
+ return -1;
+
+ /* Enable slave 0, 8-byte reads. */
+ data[0] = BIT_SLAVE_EN | 8;
+ if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
+ return -1;
+
+ /* Slave 1 changes AKM measurement mode. */
+ data[0] = st.chip_cfg.compass_addr;
+ if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
+ return -1;
+
+ /* AKM measurement mode register. */
+ data[0] = AKM_REG_CNTL;
+ if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
+ return -1;
+
+ /* Enable slave 1, 1-byte writes. */
+ data[0] = BIT_SLAVE_EN | 1;
+ if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
+ return -1;
+
+ /* Set slave 1 data. */
+ data[0] = AKM_SINGLE_MEASUREMENT;
+ if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
+ return -1;
+
+ /* Trigger slave 0 and slave 1 actions at each sample. */
+ data[0] = 0x03;
+ if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
+ return -1;
+
+#ifdef MPU9150
+ /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
+ data[0] = BIT_I2C_MST_VDDIO;
+ if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
+ return -1;
+#endif
+
+ return 0;
+#else
+ return -1;
+#endif
+}
+
+/**
+ * @brief Read raw compass data.
+ * @param[out] data Raw data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds. Null if not needed.
+ * @return 0 if successful.
+ */
+int mpu_get_compass_reg(short *data, unsigned long *timestamp)
+{
+#ifdef AK89xx_SECONDARY
+ unsigned char tmp[9];
+
+ if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
+ return -1;
+
+#ifdef AK89xx_BYPASS
+ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
+ return -1;
+ tmp[8] = AKM_SINGLE_MEASUREMENT;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
+ return -1;
+#else
+ if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
+ return -1;
+#endif
+
+#if defined AK8975_SECONDARY
+ /* AK8975 doesn't have the overrun error bit. */
+ if (!(tmp[0] & AKM_DATA_READY))
+ return -2;
+ if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
+ return -3;
+#elif defined AK8963_SECONDARY
+ /* AK8963 doesn't have the data read error bit. */
+ if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
+ return -2;
+ if (tmp[7] & AKM_OVERFLOW)
+ return -3;
+#endif
+ data[0] = (tmp[2] << 8) | tmp[1];
+ data[1] = (tmp[4] << 8) | tmp[3];
+ data[2] = (tmp[6] << 8) | tmp[5];
+
+ data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
+ data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
+ data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
+
+ if (timestamp)
+ get_ms(timestamp);
+ return 0;
+#else
+ return -1;
+#endif
+}
+
+/**
+ * @brief Get the compass full-scale range.
+ * @param[out] fsr Current full-scale range.
+ * @return 0 if successful.
+ */
+int mpu_get_compass_fsr(unsigned short *fsr)
+{
+#ifdef AK89xx_SECONDARY
+ fsr[0] = st.hw->compass_fsr;
+ return 0;
+#else
+ return -1;
+#endif
+}
+
+/**
+ * @brief Enters LP accel motion interrupt mode.
+ * The behavior of this feature is very different between the MPU6050 and the
+ * MPU6500. Each chip's version of this feature is explained below.
+ *
+ * \n MPU6050:
+ * \n When this mode is first enabled, the hardware captures a single accel
+ * sample, and subsequent samples are compared with this one to determine if
+ * the device is in motion. Therefore, whenever this "locked" sample needs to
+ * be changed, this function must be called again.
+ *
+ * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
+ * increments.
+ *
+ * \n Low-power accel mode supports the following frequencies:
+ * \n 1.25Hz, 5Hz, 20Hz, 40Hz
+ *
+ * \n MPU6500:
+ * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
+ * sample. The hardware monitors the accel data and detects any large change
+ * over a short period of time.
+ *
+ * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
+ * increments.
+ *
+ * \n MPU6500 Low-power accel mode supports the following frequencies:
+ * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
+ *
+ * \n\n NOTES:
+ * \n The driver will round down @e thresh to the nearest supported value if
+ * an unsupported threshold is selected.
+ * \n To select a fractional wake-up frequency, round down the value passed to
+ * @e lpa_freq.
+ * \n The MPU6500 does not support a delay parameter. If this function is used
+ * for the MPU6500, the value passed to @e time will be ignored.
+ * \n To disable this mode, set @e lpa_freq to zero. The driver will restore
+ * the previous configuration.
+ *
+ * @param[in] thresh Motion threshold in mg.
+ * @param[in] time Duration in milliseconds that the accel data must
+ * exceed @e thresh before motion is reported.
+ * @param[in] lpa_freq Minimum sampling rate, or zero to disable.
+ * @return 0 if successful.
+ */
+int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
+ unsigned char lpa_freq)
+{
+ unsigned char data[3];
+
+ if (lpa_freq) {
+ unsigned char thresh_hw;
+
+#if defined MPU6050
+ /* TODO: Make these const/#defines. */
+ /* 1LSb = 32mg. */
+ if (thresh > 8160)
+ thresh_hw = 255;
+ else if (thresh < 32)
+ thresh_hw = 1;
+ else
+ thresh_hw = thresh >> 5;
+#elif defined MPU6500
+ /* 1LSb = 4mg. */
+ if (thresh > 1020)
+ thresh_hw = 255;
+ else if (thresh < 4)
+ thresh_hw = 1;
+ else
+ thresh_hw = thresh >> 2;
+#endif
+
+ if (!time)
+ /* Minimum duration must be 1ms. */
+ time = 1;
+
+#if defined MPU6050
+ if (lpa_freq > 40)
+#elif defined MPU6500
+ if (lpa_freq > 640)
+#endif
+ /* At this point, the chip has not been re-configured, so the
+ * function can safely exit.
+ */
+ return -1;
+
+ if (!st.chip_cfg.int_motion_only) {
+ /* Store current settings for later. */
+ if (st.chip_cfg.dmp_on) {
+ mpu_set_dmp_state(0);
+ st.chip_cfg.cache.dmp_on = 1;
+ } else
+ st.chip_cfg.cache.dmp_on = 0;
+ mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
+ mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
+ mpu_get_lpf(&st.chip_cfg.cache.lpf);
+ mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
+ st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
+ mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
+ }
+
+#ifdef MPU6050
+ /* Disable hardware interrupts for now. */
+ set_int_enable(0);
+
+ /* Enter full-power accel-only mode. */
+ mpu_lp_accel_mode(0);
+
+ /* Override current LPF (and HPF) settings to obtain a valid accel
+ * reading.
+ */
+ data[0] = INV_FILTER_256HZ_NOLPF2;
+ if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
+ return -1;
+
+ /* NOTE: Digital high pass filter should be configured here. Since this
+ * driver doesn't modify those bits anywhere, they should already be
+ * cleared by default.
+ */
+
+ /* Configure the device to send motion interrupts. */
+ /* Enable motion interrupt. */
+ data[0] = BIT_MOT_INT_EN;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
+ goto lp_int_restore;
+
+ /* Set motion interrupt parameters. */
+ data[0] = thresh_hw;
+ data[1] = time;
+ if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data))
+ goto lp_int_restore;
+
+ /* Force hardware to "lock" current accel sample. */
+ delay_ms(5);
+ data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF;
+ if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
+ goto lp_int_restore;
+
+ /* Set up LP accel mode. */
+ data[0] = BIT_LPA_CYCLE;
+ if (lpa_freq == 1)
+ data[1] = INV_LPA_1_25HZ;
+ else if (lpa_freq <= 5)
+ data[1] = INV_LPA_5HZ;
+ else if (lpa_freq <= 20)
+ data[1] = INV_LPA_20HZ;
+ else
+ data[1] = INV_LPA_40HZ;
+ data[1] = (data[1] << 6) | BIT_STBY_XYZG;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
+ goto lp_int_restore;
+
+ st.chip_cfg.int_motion_only = 1;
+ return 0;
+#elif defined MPU6500
+ /* Disable hardware interrupts. */
+ set_int_enable(0);
+
+ /* Enter full-power accel-only mode, no FIFO/DMP. */
+ data[0] = 0;
+ data[1] = 0;
+ data[2] = BIT_STBY_XYZG;
+ if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
+ goto lp_int_restore;
+
+ /* Set motion threshold. */
+ data[0] = thresh_hw;
+ if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
+ goto lp_int_restore;
+
+ /* Set wake frequency. */
+ if (lpa_freq == 1)
+ data[0] = INV_LPA_1_25HZ;
+ else if (lpa_freq == 2)
+ data[0] = INV_LPA_2_5HZ;
+ else if (lpa_freq <= 5)
+ data[0] = INV_LPA_5HZ;
+ else if (lpa_freq <= 10)
+ data[0] = INV_LPA_10HZ;
+ else if (lpa_freq <= 20)
+ data[0] = INV_LPA_20HZ;
+ else if (lpa_freq <= 40)
+ data[0] = INV_LPA_40HZ;
+ else if (lpa_freq <= 80)
+ data[0] = INV_LPA_80HZ;
+ else if (lpa_freq <= 160)
+ data[0] = INV_LPA_160HZ;
+ else if (lpa_freq <= 320)
+ data[0] = INV_LPA_320HZ;
+ else
+ data[0] = INV_LPA_640HZ;
+ if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
+ goto lp_int_restore;
+
+ /* Enable motion interrupt (MPU6500 version). */
+ data[0] = BITS_WOM_EN;
+ if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
+ goto lp_int_restore;
+
+ /* Enable cycle mode. */
+ data[0] = BIT_LPA_CYCLE;
+ if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
+ goto lp_int_restore;
+
+ /* Enable interrupt. */
+ data[0] = BIT_MOT_INT_EN;
+ if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
+ goto lp_int_restore;
+
+ st.chip_cfg.int_motion_only = 1;
+ return 0;
+#endif
+ } else {
+ /* Don't "restore" the previous state if no state has been saved. */
+ int ii;
+ char *cache_ptr = (char*)&st.chip_cfg.cache;
+ for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
+ if (cache_ptr[ii] != 0)
+ goto lp_int_restore;
+ }
+ /* If we reach this point, motion interrupt mode hasn't been used yet. */
+ return -1;
+ }
+lp_int_restore:
+ /* Set to invalid values to ensure no I2C writes are skipped. */
+ st.chip_cfg.gyro_fsr = 0xFF;
+ st.chip_cfg.accel_fsr = 0xFF;
+ st.chip_cfg.lpf = 0xFF;
+ st.chip_cfg.sample_rate = 0xFFFF;
+ st.chip_cfg.sensors = 0xFF;
+ st.chip_cfg.fifo_enable = 0xFF;
+ st.chip_cfg.clk_src = INV_CLK_PLL;
+ mpu_set_sensors(st.chip_cfg.cache.sensors_on);
+ mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
+ mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
+ mpu_set_lpf(st.chip_cfg.cache.lpf);
+ mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
+ mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
+
+ if (st.chip_cfg.cache.dmp_on)
+ mpu_set_dmp_state(1);
+
+#ifdef MPU6500
+ /* Disable motion interrupt (MPU6500 version). */
+ data[0] = 0;
+ if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
+ goto lp_int_restore;
+#endif
+
+ st.chip_cfg.int_motion_only = 0;
+ return 0;
+}
+//////////////////////////////////////////////////////////////////////////////////
+//添加的代码部分
+//////////////////////////////////////////////////////////////////////////////////
+//本程序只供学习使用，未经作者许可，不得用于其它任何用途
+//ALIENTEK STM32开发板
+//MPU6050 DMP 驱动代码
+//正点原子@ALIENTEK
+//技术论坛:www.openedv.com
+//创建日期:2015/1/17
+//版本：V1.0
+//版权所有，盗版必究。
+//Copyright(C) 广州市星翼电子科技有限公司 2009-2019
+//All rights reserved
+//////////////////////////////////////////////////////////////////////////////////
+
+//q30格式,long转float时的除数.
+#define q30 1073741824.0f
+
+//陀螺仪方向设置
+static signed char gyro_orientation[9] = { 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1};
+//MPU6050自测试
+//返回值:0,正常
+// 其他,失败
+unsigned char run_self_test(void)
+{
+ int result;
+ //char test_packet[4] = {0};
+ long gyro[3], accel[3];
+ result = mpu_run_self_test(gyro, accel);
+ if (result == 0x3)
+ {
+ /* Test passed. We can trust the gyro data here, so let's push it down
+ * to the DMP.
+ */
+ float sens;
+ unsigned short accel_sens;
+ mpu_get_gyro_sens(&sens);
+ gyro[0] = (long)(gyro[0] * sens);
+ gyro[1] = (long)(gyro[1] * sens);
+ gyro[2] = (long)(gyro[2] * sens);
+ dmp_set_gyro_bias(gyro);
+ mpu_get_accel_sens(&accel_sens);
+ accel[0] *= accel_sens;
+ accel[1] *= accel_sens;
+ accel[2] *= accel_sens;
+ dmp_set_accel_bias(accel);
+ return 0;
+ }else return 1;
+}
+//陀螺仪方向控制
+unsigned short inv_orientation_matrix_to_scalar(
+ const signed char *mtx)
+{
+ unsigned short scalar;
+ /*
+ XYZ 010_001_000 Identity Matrix
+ XZY 001_010_000
+ YXZ 010_000_001
+ YZX 000_010_001
+ ZXY 001_000_010
+ ZYX 000_001_010
+ */
+
+ scalar = inv_row_2_scale(mtx);
+ scalar |= inv_row_2_scale(mtx + 3) << 3;
+ scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+
+ return scalar;
+}
+//方向转换
+unsigned short inv_row_2_scale(const signed char *row)
+{
+ unsigned short b;
+
+ if (row[0] > 0)
+ b = 0;
+ else if (row[0] < 0)
+ b = 4;
+ else if (row[1] > 0)
+ b = 1;
+ else if (row[1] < 0)
+ b = 5;
+ else if (row[2] > 0)
+ b = 2;
+ else if (row[2] < 0)
+ b = 6;
+ else
+ b = 7; // error
+ return b;
+}
+//空函数,未用到.
+void mget_ms(unsigned long *time)
+{
+
+}
+//mpu6050,dmp初始化
+//返回值:0,正常
+// 其他,失败
+unsigned char mpu_dmp_init(void)
+{
+ unsigned char res=0;
+ MPU_IIC_Init(); //初始化IIC总线
+ if(mpu_init()==0) //初始化MPU6050
+ {
+ res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
+ if(res)return 1;
+ res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
+ if(res)return 2;
+ res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
+ if(res)return 3;
+ res=dmp_load_motion_driver_firmware(); //加载dmp固件
+ if(res)return 4;
+ res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
+ if(res)return 5;
+ res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
+ DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
+ DMP_FEATURE_GYRO_CAL);
+ if(res)return 6;
+ res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
+ if(res)return 7;
+ res=run_self_test(); //自检
+ if(res)return 8;
+ res=mpu_set_dmp_state(1); //使能DMP
+ if(res)return 9;
+ }else return 10;
+ return 0;
+}
+//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
+//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
+//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
+//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
+//返回值:0,正常
+// 其他,失败
+unsigned char mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
+{
+ float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
+ unsigned long sensor_timestamp;
+ short gyro[3], accel[3], sensors;
+ unsigned char more;
+ long quat[4];
+ if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
+ /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
+ * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
+ **/
+ /*if (sensors & INV_XYZ_GYRO )
+ send_packet(PACKET_TYPE_GYRO, gyro);
+ if (sensors & INV_XYZ_ACCEL)
+ send_packet(PACKET_TYPE_ACCEL, accel); */
+ /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
+ * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
+ **/
+ if(sensors&INV_WXYZ_QUAT)
+ {
+ q0 = quat[0] / q30; //q30格式转换为浮点数
+ q1 = quat[1] / q30;
+ q2 = quat[2] / q30;
+ q3 = quat[3] / q30;
+ //计算得到俯仰角/横滚角/航向角
+ *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
+ *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
+ *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
+ }else return 2;
+ return 0;
+}
+
+//定义目标板采用MSP430
+#define MOTION_DRIVER_TARGET_MSP430
+
+/* The following functions must be defined for this platform:
+ * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
+ * unsigned char length, unsigned char const *data)
+ * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
+ * unsigned char length, unsigned char *data)
+ * delay_ms(unsigned long num_ms)
+ * get_ms(unsigned long *count)
+ */
+#if defined MOTION_DRIVER_TARGET_MSP430
+//#include "msp430.h"
+//#include "msp430_clock.h"
+//#define delay_ms delay_ms
+#define get_ms mget_ms
+//#define // printf
+//#define // printf
+
+#elif defined EMPL_TARGET_MSP430
+#include "msp430.h"
+#include "msp430_clock.h"
+#include "log.h"
+#define delay_ms msp430_delay_ms
+#define get_ms msp430_get_clock_ms
+//#define // MPL_LOGI
+//#define // MPL_LOGE
+
+#elif defined EMPL_TARGET_UC3L0
+/* Instead of using the standard TWI driver from the ASF library, we're using
+ * a TWI driver that follows the slave address + register address convention.
+ */
+#include "delay.h"
+#include "sysclk.h"
+#include "log.h"
+#include "uc3l0_clock.h"
+/* delay_ms is a function already defined in ASF. */
+#define get_ms uc3l0_get_clock_ms
+//#define // MPL_LOGI
+//#define // MPL_LOGE
+
+#else
+#error Gyro driver is missing the system layer implementations.
+#endif
+
+/* These defines are copied from dmpDefaultMPU6050.c in the general MPL
+ * releases. These defines may change for each DMP image, so be sure to modify
+ * these values when switching to a new image.
+ */
+#define CFG_LP_QUAT (2712)
+#define END_ORIENT_TEMP (1866)
+#define CFG_27 (2742)
+#define CFG_20 (2224)
+#define CFG_23 (2745)
+#define CFG_FIFO_ON_EVENT (2690)
+#define END_PREDICTION_UPDATE (1761)
+#define CGNOTICE_INTR (2620)
+#define X_GRT_Y_TMP (1358)
+#define CFG_DR_INT (1029)
+#define CFG_AUTH (1035)
+#define UPDATE_PROP_ROT (1835)
+#define END_COMPARE_Y_X_TMP2 (1455)
+#define SKIP_X_GRT_Y_TMP (1359)
+#define SKIP_END_COMPARE (1435)
+#define FCFG_3 (1088)
+#define FCFG_2 (1066)
+#define FCFG_1 (1062)
+#define END_COMPARE_Y_X_TMP3 (1434)
+#define FCFG_7 (1073)
+#define FCFG_6 (1106)
+#define FLAT_STATE_END (1713)
+#define SWING_END_4 (1616)
+#define SWING_END_2 (1565)
+#define SWING_END_3 (1587)
+#define SWING_END_1 (1550)
+#define CFG_8 (2718)
+#define CFG_15 (2727)
+#define CFG_16 (2746)
+#define CFG_EXT_GYRO_BIAS (1189)
+#define END_COMPARE_Y_X_TMP (1407)
+#define DO_NOT_UPDATE_PROP_ROT (1839)
+#define CFG_7 (1205)
+#define FLAT_STATE_END_TEMP (1683)
+#define END_COMPARE_Y_X (1484)
+#define SKIP_SWING_END_1 (1551)
+#define SKIP_SWING_END_3 (1588)
+#define SKIP_SWING_END_2 (1566)
+#define TILTG75_START (1672)
+#define CFG_6 (2753)
+#define TILTL75_END (1669)
+#define END_ORIENT (1884)
+#define CFG_FLICK_IN (2573)
+#define TILTL75_START (1643)
+#define CFG_MOTION_BIAS (1208)
+#define X_GRT_Y (1408)
+#define TEMPLABEL (2324)
+#define CFG_ANDROID_ORIENT_INT (1853)
+#define CFG_GYRO_RAW_DATA (2722)
+#define X_GRT_Y_TMP2 (1379)
+
+#define D_0_22 (22+512)
+#define D_0_24 (24+512)
+
+#define D_0_36 (36)
+#define D_0_52 (52)
+#define D_0_96 (96)
+#define D_0_104 (104)
+#define D_0_108 (108)
+#define D_0_163 (163)
+#define D_0_188 (188)
+#define D_0_192 (192)
+#define D_0_224 (224)
+#define D_0_228 (228)
+#define D_0_232 (232)
+#define D_0_236 (236)
+
+#define D_1_2 (256 + 2)
+#define D_1_4 (256 + 4)
+#define D_1_8 (256 + 8)
+#define D_1_10 (256 + 10)
+#define D_1_24 (256 + 24)
+#define D_1_28 (256 + 28)
+#define D_1_36 (256 + 36)
+#define D_1_40 (256 + 40)
+#define D_1_44 (256 + 44)
+#define D_1_72 (256 + 72)
+#define D_1_74 (256 + 74)
+#define D_1_79 (256 + 79)
+#define D_1_88 (256 + 88)
+#define D_1_90 (256 + 90)
+#define D_1_92 (256 + 92)
+#define D_1_96 (256 + 96)
+#define D_1_98 (256 + 98)
+#define D_1_106 (256 + 106)
+#define D_1_108 (256 + 108)
+#define D_1_112 (256 + 112)
+#define D_1_128 (256 + 144)
+#define D_1_152 (256 + 12)
+#define D_1_160 (256 + 160)
+#define D_1_176 (256 + 176)
+#define D_1_178 (256 + 178)
+#define D_1_218 (256 + 218)
+#define D_1_232 (256 + 232)
+#define D_1_236 (256 + 236)
+#define D_1_240 (256 + 240)
+#define D_1_244 (256 + 244)
+#define D_1_250 (256 + 250)
+#define D_1_252 (256 + 252)
+#define D_2_12 (512 + 12)
+#define D_2_96 (512 + 96)
+#define D_2_108 (512 + 108)
+#define D_2_208 (512 + 208)
+#define D_2_224 (512 + 224)
+#define D_2_236 (512 + 236)
+#define D_2_244 (512 + 244)
+#define D_2_248 (512 + 248)
+#define D_2_252 (512 + 252)
+
+#define CPASS_BIAS_X (35 * 16 + 4)
+#define CPASS_BIAS_Y (35 * 16 + 8)
+#define CPASS_BIAS_Z (35 * 16 + 12)
+#define CPASS_MTX_00 (36 * 16)
+#define CPASS_MTX_01 (36 * 16 + 4)
+#define CPASS_MTX_02 (36 * 16 + 8)
+#define CPASS_MTX_10 (36 * 16 + 12)
+#define CPASS_MTX_11 (37 * 16)
+#define CPASS_MTX_12 (37 * 16 + 4)
+#define CPASS_MTX_20 (37 * 16 + 8)
+#define CPASS_MTX_21 (37 * 16 + 12)
+#define CPASS_MTX_22 (43 * 16 + 12)
+#define D_EXT_GYRO_BIAS_X (61 * 16)
+#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4
+#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8
+#define D_ACT0 (40 * 16)
+#define D_ACSX (40 * 16 + 4)
+#define D_ACSY (40 * 16 + 8)
+#define D_ACSZ (40 * 16 + 12)
+
+#define FLICK_MSG (45 * 16 + 4)
+#define FLICK_COUNTER (45 * 16 + 8)
+#define FLICK_LOWER (45 * 16 + 12)
+#define FLICK_UPPER (46 * 16 + 12)
+
+#define D_AUTH_OUT (992)
+#define D_AUTH_IN (996)
+#define D_AUTH_A (1000)
+#define D_AUTH_B (1004)
+
+#define D_PEDSTD_BP_B (768 + 0x1C)
+#define D_PEDSTD_HP_A (768 + 0x78)
+#define D_PEDSTD_HP_B (768 + 0x7C)
+#define D_PEDSTD_BP_A4 (768 + 0x40)
+#define D_PEDSTD_BP_A3 (768 + 0x44)
+#define D_PEDSTD_BP_A2 (768 + 0x48)
+#define D_PEDSTD_BP_A1 (768 + 0x4C)
+#define D_PEDSTD_INT_THRSH (768 + 0x68)
+#define D_PEDSTD_CLIP (768 + 0x6C)
+#define D_PEDSTD_SB (768 + 0x28)
+#define D_PEDSTD_SB_TIME (768 + 0x2C)
+#define D_PEDSTD_PEAKTHRSH (768 + 0x98)
+#define D_PEDSTD_TIML (768 + 0x2A)
+#define D_PEDSTD_TIMH (768 + 0x2E)
+#define D_PEDSTD_PEAK (768 + 0X94)
+#define D_PEDSTD_STEPCTR (768 + 0x60)
+#define D_PEDSTD_TIMECTR (964)
+#define D_PEDSTD_DECI (768 + 0xA0)
+
+#define D_HOST_NO_MOT (976)
+#define D_ACCEL_BIAS (660)
+
+#define D_ORIENT_GAP (76)
+
+#define D_TILT0_H (48)
+#define D_TILT0_L (50)
+#define D_TILT1_H (52)
+#define D_TILT1_L (54)
+#define D_TILT2_H (56)
+#define D_TILT2_L (58)
+#define D_TILT3_H (60)
+#define D_TILT3_L (62)
+
+#define DMP_CODE_SIZE (3062)
+
+static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
+ /* bank # 0 */
+ 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
+ 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
+ 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
+ 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
+ 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
+ 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
+ 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
+ 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+ 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
+ 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
+ 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
+ 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
+ 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
+ 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
+ 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ /* bank # 1 */
+ 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
+ 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
+ 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
+ 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
+ 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
+ 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+ 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
+ 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
+ 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+ 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
+ /* bank # 2 */
+ 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
+ 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+ 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
+ 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
+ 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
+ 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ /* bank # 3 */
+ 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
+ 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+ /* bank # 4 */
+ 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
+ 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
+ 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
+ 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
+ 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
+ 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
+ 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
+ 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
+ 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
+ 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
+ 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
+ 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
+ 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
+ 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
+ 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
+ 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
+ /* bank # 5 */
+ 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
+ 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
+ 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
+ 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
+ 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
+ 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
+ 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
+ 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
+ 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
+ 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
+ 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
+ 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
+ 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
+ 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
+ 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
+ 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
+ /* bank # 6 */
+ 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
+ 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
+ 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
+ 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
+ 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
+ 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
+ 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
+ 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
+ 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
+ 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
+ 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
+ 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
+ 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
+ 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
+ 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
+ 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
+ /* bank # 7 */
+ 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
+ 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
+ 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
+ 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
+ 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
+ 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
+ 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
+ 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
+ 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
+ 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
+ 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
+ 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
+ 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
+ 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
+ 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
+ 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
+ /* bank # 8 */
+ 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
+ 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
+ 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
+ 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
+ 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
+ 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
+ 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
+ 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
+ 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
+ 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
+ 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
+ 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
+ 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
+ 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
+ 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
+ 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
+ /* bank # 9 */
+ 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
+ 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
+ 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
+ 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
+ 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
+ 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
+ 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
+ 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
+ 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
+ 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
+ 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
+ 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
+ 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
+ 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
+ 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
+ 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
+ /* bank # 10 */
+ 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
+ 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
+ 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
+ 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
+ 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
+ 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
+ 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
+ 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
+ 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
+ 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
+ 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
+ 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
+ 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
+ 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
+ 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
+ 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
+ /* bank # 11 */
+ 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
+ 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
+ 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
+ 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
+ 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
+ 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
+ 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
+ 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
+ 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
+ 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
+ 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
+ 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
+ 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
+ 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
+ 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
+ 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
+};
+
+static const unsigned short sStartAddress = 0x0400;
+
+/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
+
+#define INT_SRC_TAP (0x01)
+#define INT_SRC_ANDROID_ORIENT (0x08)
+
+#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \
+ DMP_FEATURE_SEND_CAL_GYRO)
+
+#define MAX_PACKET_LENGTH_2 (32) //前面已经有一个定义了你，避免冲突改成2
+
+#define DMP_SAMPLE_RATE (200)
+#define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE)
+
+#define FIFO_CORRUPTION_CHECK
+#ifdef FIFO_CORRUPTION_CHECK
+#define QUAT_ERROR_THRESH (1L<<24)
+#define QUAT_MAG_SQ_NORMALIZED (1L<<28)
+#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
+#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
+#endif
+
+struct dmp_s {
+ void (*tap_cb)(unsigned char count, unsigned char direction);
+ void (*android_orient_cb)(unsigned char orientation);
+ unsigned short orient;
+ unsigned short feature_mask;
+ unsigned short fifo_rate;
+ unsigned char packet_length;
+};
+
+//static struct dmp_s dmp = {
+// .tap_cb = NULL,
+// .android_orient_cb = NULL,
+// .orient = 0,
+// .feature_mask = 0,
+// .fifo_rate = 0,
+// .packet_length = 0
+//};
+
+static struct dmp_s dmp={
+ NULL,
+ NULL,
+ 0,
+ 0,
+ 0,
+ 0
+};
+
+/**
+ * @brief Load the DMP with this image.
+ * @return 0 if successful.
+ */
+int dmp_load_motion_driver_firmware(void)
+{
+ return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
+ DMP_SAMPLE_RATE);
+}
+
+/**
+ * @brief Push gyro and accel orientation to the DMP.
+ * The orientation is represented here as the output of
+ * @e inv_orientation_matrix_to_scalar.
+ * @param[in] orient Gyro and accel orientation in body frame.
+ * @return 0 if successful.
+ */
+int dmp_set_orientation(unsigned short orient)
+{
+ unsigned char gyro_regs[3], accel_regs[3];
+ const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
+ const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
+ const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
+ const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
+
+ gyro_regs[0] = gyro_axes[orient & 3];
+ gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
+ gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
+ accel_regs[0] = accel_axes[orient & 3];
+ accel_regs[1] = accel_axes[(orient >> 3) & 3];
+ accel_regs[2] = accel_axes[(orient >> 6) & 3];
+
+ /* Chip-to-body, axes only. */
+ if (mpu_write_mem(FCFG_1, 3, gyro_regs))
+ return -1;
+ if (mpu_write_mem(FCFG_2, 3, accel_regs))
+ return -1;
+
+ memcpy(gyro_regs, gyro_sign, 3);
+ memcpy(accel_regs, accel_sign, 3);
+ if (orient & 4) {
+ gyro_regs[0] |= 1;
+ accel_regs[0] |= 1;
+ }
+ if (orient & 0x20) {
+ gyro_regs[1] |= 1;
+ accel_regs[1] |= 1;
+ }
+ if (orient & 0x100) {
+ gyro_regs[2] |= 1;
+ accel_regs[2] |= 1;
+ }
+
+ /* Chip-to-body, sign only. */
+ if (mpu_write_mem(FCFG_3, 3, gyro_regs))
+ return -1;
+ if (mpu_write_mem(FCFG_7, 3, accel_regs))
+ return -1;
+ dmp.orient = orient;
+ return 0;
+}
+
+/**
+ * @brief Push gyro biases to the DMP.
+ * Because the gyro integration is handled in the DMP, any gyro biases
+ * calculated by the MPL should be pushed down to DMP memory to remove
+ * 3-axis quaternion drift.
+ * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
+ * overwrite the biases written to this location once a new one is computed.
+ * @param[in] bias Gyro biases in q16.
+ * @return 0 if successful.
+ */
+int dmp_set_gyro_bias(long *bias)
+{
+ long gyro_bias_body[3];
+ unsigned char regs[4];
+
+ gyro_bias_body[0] = bias[dmp.orient & 3];
+ if (dmp.orient & 4)
+ gyro_bias_body[0] *= -1;
+ gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
+ if (dmp.orient & 0x20)
+ gyro_bias_body[1] *= -1;
+ gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
+ if (dmp.orient & 0x100)
+ gyro_bias_body[2] *= -1;
+
+#ifdef EMPL_NO_64BIT
+ gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
+ gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
+ gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
+#else
+ gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
+ gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
+ gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
+#endif
+
+ regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
+ regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
+ regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
+ regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
+ if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
+ return -1;
+
+ regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
+ regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
+ regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
+ regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
+ if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
+ return -1;
+
+ regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
+ regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
+ regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
+ regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
+ return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
+}
+
+/**
+ * @brief Push accel biases to the DMP.
+ * These biases will be removed from the DMP 6-axis quaternion.
+ * @param[in] bias Accel biases in q16.
+ * @return 0 if successful.
+ */
+int dmp_set_accel_bias(long *bias)
+{
+ long accel_bias_body[3];
+ unsigned char regs[12];
+ long long accel_sf;
+ unsigned short accel_sens;
+
+ mpu_get_accel_sens(&accel_sens);
+ accel_sf = (long long)accel_sens << 15;
+ //__no_operation();
+
+ accel_bias_body[0] = bias[dmp.orient & 3];
+ if (dmp.orient & 4)
+ accel_bias_body[0] *= -1;
+ accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
+ if (dmp.orient & 0x20)
+ accel_bias_body[1] *= -1;
+ accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
+ if (dmp.orient & 0x100)
+ accel_bias_body[2] *= -1;
+
+#ifdef EMPL_NO_64BIT
+ accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
+ accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
+ accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
+#else
+ accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
+ accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
+ accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
+#endif
+
+ regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
+ regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
+ regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
+ regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
+ regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
+ regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
+ regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
+ regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
+ regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
+ regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
+ regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
+ regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
+ return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
+}
+
+/**
+ * @brief Set DMP output rate.
+ * Only used when DMP is on.
+ * @param[in] rate Desired fifo rate (Hz).
+ * @return 0 if successful.
+ */
+int dmp_set_fifo_rate(unsigned short rate)
+{
+ const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
+ 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
+ unsigned short div;
+ unsigned char tmp[8];
+
+ if (rate > DMP_SAMPLE_RATE)
+ return -1;
+ div = DMP_SAMPLE_RATE / rate - 1;
+ tmp[0] = (unsigned char)((div >> 8) & 0xFF);
+ tmp[1] = (unsigned char)(div & 0xFF);
+ if (mpu_write_mem(D_0_22, 2, tmp))
+ return -1;
+ if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
+ return -1;
+
+ dmp.fifo_rate = rate;
+ return 0;
+}
+
+/**
+ * @brief Get DMP output rate.
+ * @param[out] rate Current fifo rate (Hz).
+ * @return 0 if successful.
+ */
+int dmp_get_fifo_rate(unsigned short *rate)
+{
+ rate[0] = dmp.fifo_rate;
+ return 0;
+}
+
+/**
+ * @brief Set tap threshold for a specific axis.
+ * @param[in] axis 1, 2, and 4 for XYZ accel, respectively.
+ * @param[in] thresh Tap threshold, in mg/ms.
+ * @return 0 if successful.
+ */
+int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
+{
+ unsigned char tmp[4], accel_fsr;
+ float scaled_thresh;
+ unsigned short dmp_thresh, dmp_thresh_2;
+ if (!(axis & TAP_XYZ) || thresh > 1600)
+ return -1;
+
+ scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
+
+ mpu_get_accel_fsr(&accel_fsr);
+ switch (accel_fsr) {
+ case 2:
+ dmp_thresh = (unsigned short)(scaled_thresh * 16384);
+ /* dmp_thresh * 0.75 */
+ dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
+ break;
+ case 4:
+ dmp_thresh = (unsigned short)(scaled_thresh * 8192);
+ /* dmp_thresh * 0.75 */
+ dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
+ break;
+ case 8:
+ dmp_thresh = (unsigned short)(scaled_thresh * 4096);
+ /* dmp_thresh * 0.75 */
+ dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
+ break;
+ case 16:
+ dmp_thresh = (unsigned short)(scaled_thresh * 2048);
+ /* dmp_thresh * 0.75 */
+ dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
+ break;
+ default:
+ return -1;
+ }
+ tmp[0] = (unsigned char)(dmp_thresh >> 8);
+ tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
+ tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
+ tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
+
+ if (axis & TAP_X) {
+ if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
+ return -1;
+ if (mpu_write_mem(D_1_36, 2, tmp+2))
+ return -1;
+ }
+ if (axis & TAP_Y) {
+ if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
+ return -1;
+ if (mpu_write_mem(D_1_40, 2, tmp+2))
+ return -1;
+ }
+ if (axis & TAP_Z) {
+ if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
+ return -1;
+ if (mpu_write_mem(D_1_44, 2, tmp+2))
+ return -1;
+ }
+ return 0;
+}
+
+/**
+ * @brief Set which axes will register a tap.
+ * @param[in] axis 1, 2, and 4 for XYZ, respectively.
+ * @return 0 if successful.
+ */
+int dmp_set_tap_axes(unsigned char axis)
+{
+ unsigned char tmp = 0;
+
+ if (axis & TAP_X)
+ tmp |= 0x30;
+ if (axis & TAP_Y)
+ tmp |= 0x0C;
+ if (axis & TAP_Z)
+ tmp |= 0x03;
+ return mpu_write_mem(D_1_72, 1, &tmp);
+}
+
+/**
+ * @brief Set minimum number of taps needed for an interrupt.
+ * @param[in] min_taps Minimum consecutive taps (1-4).
+ * @return 0 if successful.
+ */
+int dmp_set_tap_count(unsigned char min_taps)
+{
+ unsigned char tmp;
+
+ if (min_taps < 1)
+ min_taps = 1;
+ else if (min_taps > 4)
+ min_taps = 4;
+
+ tmp = min_taps - 1;
+ return mpu_write_mem(D_1_79, 1, &tmp);
+}
+
+/**
+ * @brief Set length between valid taps.
+ * @param[in] time Milliseconds between taps.
+ * @return 0 if successful.
+ */
+int dmp_set_tap_time(unsigned short time)
+{
+ unsigned short dmp_time;
+ unsigned char tmp[2];
+
+ dmp_time = time / (1000 / DMP_SAMPLE_RATE);
+ tmp[0] = (unsigned char)(dmp_time >> 8);
+ tmp[1] = (unsigned char)(dmp_time & 0xFF);
+ return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
+}
+
+/**
+ * @brief Set max time between taps to register as a multi-tap.
+ * @param[in] time Max milliseconds between taps.
+ * @return 0 if successful.
+ */
+int dmp_set_tap_time_multi(unsigned short time)
+{
+ unsigned short dmp_time;
+ unsigned char tmp[2];
+
+ dmp_time = time / (1000 / DMP_SAMPLE_RATE);
+ tmp[0] = (unsigned char)(dmp_time >> 8);
+ tmp[1] = (unsigned char)(dmp_time & 0xFF);
+ return mpu_write_mem(D_1_218, 2, tmp);
+}
+
+/**
+ * @brief Set shake rejection threshold.
+ * If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
+ * @param[in] sf Gyro scale factor.
+ * @param[in] thresh Gyro threshold in dps.
+ * @return 0 if successful.
+ */
+int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
+{
+ unsigned char tmp[4];
+ long thresh_scaled = sf / 1000 * thresh;
+ tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
+ tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
+ tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
+ tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
+ return mpu_write_mem(D_1_92, 4, tmp);
+}
+
+/**
+ * @brief Set shake rejection time.
+ * Sets the length of time that the gyro must be outside of the threshold set
+ * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
+ * 60 ms is added to this parameter.
+ * @param[in] time Time in milliseconds.
+ * @return 0 if successful.
+ */
+int dmp_set_shake_reject_time(unsigned short time)
+{
+ unsigned char tmp[2];
+
+ time /= (1000 / DMP_SAMPLE_RATE);
+ tmp[0] = time >> 8;
+ tmp[1] = time & 0xFF;
+ return mpu_write_mem(D_1_90,2,tmp);
+}
+
+/**
+ * @brief Set shake rejection timeout.
+ * Sets the length of time after a shake rejection that the gyro must stay
+ * inside of the threshold before taps can be detected again. A mandatory
+ * 60 ms is added to this parameter.
+ * @param[in] time Time in milliseconds.
+ * @return 0 if successful.
+ */
+int dmp_set_shake_reject_timeout(unsigned short time)
+{
+ unsigned char tmp[2];
+
+ time /= (1000 / DMP_SAMPLE_RATE);
+ tmp[0] = time >> 8;
+ tmp[1] = time & 0xFF;
+ return mpu_write_mem(D_1_88,2,tmp);
+}
+
+/**
+ * @brief Get current step count.
+ * @param[out] count Number of steps detected.
+ * @return 0 if successful.
+ */
+int dmp_get_pedometer_step_count(unsigned long *count)
+{
+ unsigned char tmp[4];
+ if (!count)
+ return -1;
+
+ if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
+ return -1;
+
+ count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
+ ((unsigned long)tmp[2] << 8) | tmp[3];
+ return 0;
+}
+
+/**
+ * @brief Overwrite current step count.
+ * WARNING: This function writes to DMP memory and could potentially encounter
+ * a race condition if called while the pedometer is enabled.
+ * @param[in] count New step count.
+ * @return 0 if successful.
+ */
+int dmp_set_pedometer_step_count(unsigned long count)
+{
+ unsigned char tmp[4];
+
+ tmp[0] = (unsigned char)((count >> 24) & 0xFF);
+ tmp[1] = (unsigned char)((count >> 16) & 0xFF);
+ tmp[2] = (unsigned char)((count >> 8) & 0xFF);
+ tmp[3] = (unsigned char)(count & 0xFF);
+ return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
+}
+
+/**
+ * @brief Get duration of walking time.
+ * @param[in] time Walk time in milliseconds.
+ * @return 0 if successful.
+ */
+int dmp_get_pedometer_walk_time(unsigned long *time)
+{
+ unsigned char tmp[4];
+ if (!time)
+ return -1;
+
+ if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
+ return -1;
+
+ time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
+ ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
+ return 0;
+}
+
+/**
+ * @brief Overwrite current walk time.
+ * WARNING: This function writes to DMP memory and could potentially encounter
+ * a race condition if called while the pedometer is enabled.
+ * @param[in] time New walk time in milliseconds.
+ */
+int dmp_set_pedometer_walk_time(unsigned long time)
+{
+ unsigned char tmp[4];
+
+ time /= 20;
+
+ tmp[0] = (unsigned char)((time >> 24) & 0xFF);
+ tmp[1] = (unsigned char)((time >> 16) & 0xFF);
+ tmp[2] = (unsigned char)((time >> 8) & 0xFF);
+ tmp[3] = (unsigned char)(time & 0xFF);
+ return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
+}
+
+/**
+ * @brief Enable DMP features.
+ * The following \#define's are used in the input mask:
+ * \n DMP_FEATURE_TAP
+ * \n DMP_FEATURE_ANDROID_ORIENT
+ * \n DMP_FEATURE_LP_QUAT
+ * \n DMP_FEATURE_6X_LP_QUAT
+ * \n DMP_FEATURE_GYRO_CAL
+ * \n DMP_FEATURE_SEND_RAW_ACCEL
+ * \n DMP_FEATURE_SEND_RAW_GYRO
+ * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
+ * exclusive.
+ * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
+ * mutually exclusive.
+ * @param[in] mask Mask of features to enable.
+ * @return 0 if successful.
+ */
+int dmp_enable_feature(unsigned short mask)
+{
+ unsigned char tmp[10];
+
+ /* TODO: All of these settings can probably be integrated into the default
+ * DMP image.
+ */
+ /* Set integration scale factor. */
+ tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
+ tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
+ tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
+ tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
+ mpu_write_mem(D_0_104, 4, tmp);
+
+ /* Send sensor data to the FIFO. */
+ tmp[0] = 0xA3;
+ if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
+ tmp[1] = 0xC0;
+ tmp[2] = 0xC8;
+ tmp[3] = 0xC2;
+ } else {
+ tmp[1] = 0xA3;
+ tmp[2] = 0xA3;
+ tmp[3] = 0xA3;
+ }
+ if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
+ tmp[4] = 0xC4;
+ tmp[5] = 0xCC;
+ tmp[6] = 0xC6;
+ } else {
+ tmp[4] = 0xA3;
+ tmp[5] = 0xA3;
+ tmp[6] = 0xA3;
+ }
+ tmp[7] = 0xA3;
+ tmp[8] = 0xA3;
+ tmp[9] = 0xA3;
+ mpu_write_mem(CFG_15,10,tmp);
+
+ /* Send gesture data to the FIFO. */
+ if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
+ tmp[0] = DINA20;
+ else
+ tmp[0] = 0xD8;
+ mpu_write_mem(CFG_27,1,tmp);
+
+ if (mask & DMP_FEATURE_GYRO_CAL)
+ dmp_enable_gyro_cal(1);
+ else
+ dmp_enable_gyro_cal(0);
+
+ if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
+ if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
+ tmp[0] = 0xB2;
+ tmp[1] = 0x8B;
+ tmp[2] = 0xB6;
+ tmp[3] = 0x9B;
+ } else {
+ tmp[0] = DINAC0;
+ tmp[1] = DINA80;
+ tmp[2] = DINAC2;
+ tmp[3] = DINA90;
+ }
+ mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
+ }
+
+ if (mask & DMP_FEATURE_TAP) {
+ /* Enable tap. */
+ tmp[0] = 0xF8;
+ mpu_write_mem(CFG_20, 1, tmp);
+ dmp_set_tap_thresh(TAP_XYZ, 250);
+ dmp_set_tap_axes(TAP_XYZ);
+ dmp_set_tap_count(1);
+ dmp_set_tap_time(100);
+ dmp_set_tap_time_multi(500);
+
+ dmp_set_shake_reject_thresh(GYRO_SF, 200);
+ dmp_set_shake_reject_time(40);
+ dmp_set_shake_reject_timeout(10);
+ } else {
+ tmp[0] = 0xD8;
+ mpu_write_mem(CFG_20, 1, tmp);
+ }
+
+ if (mask & DMP_FEATURE_ANDROID_ORIENT) {
+ tmp[0] = 0xD9;
+ } else
+ tmp[0] = 0xD8;
+ mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
+
+ if (mask & DMP_FEATURE_LP_QUAT)
+ dmp_enable_lp_quat(1);
+ else
+ dmp_enable_lp_quat(0);
+
+ if (mask & DMP_FEATURE_6X_LP_QUAT)
+ dmp_enable_6x_lp_quat(1);
+ else
+ dmp_enable_6x_lp_quat(0);
+
+ /* Pedometer is always enabled. */
+ dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
+ mpu_reset_fifo();
+
+ dmp.packet_length = 0;
+ if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
+ dmp.packet_length += 6;
+ if (mask & DMP_FEATURE_SEND_ANY_GYRO)
+ dmp.packet_length += 6;
+ if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
+ dmp.packet_length += 16;
+ if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
+ dmp.packet_length += 4;
+
+ return 0;
+}
+
+/**
+ * @brief Get list of currently enabled DMP features.
+ * @param[out] Mask of enabled features.
+ * @return 0 if successful.
+ */
+int dmp_get_enabled_features(unsigned short *mask)
+{
+ mask[0] = dmp.feature_mask;
+ return 0;
+}
+
+/**
+ * @brief Calibrate the gyro data in the DMP.
+ * After eight seconds of no motion, the DMP will compute gyro biases and
+ * subtract them from the quaternion output. If @e dmp_enable_feature is
+ * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
+ * subtracted from the gyro output.
+ * @param[in] enable 1 to enable gyro calibration.
+ * @return 0 if successful.
+ */
+int dmp_enable_gyro_cal(unsigned char enable)
+{
+ if (enable) {
+ unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
+ return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
+ } else {
+ unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
+ return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
+ }
+}
+
+/**
+ * @brief Generate 3-axis quaternions from the DMP.
+ * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
+ * exclusive.
+ * @param[in] enable 1 to enable 3-axis quaternion.
+ * @return 0 if successful.
+ */
+int dmp_enable_lp_quat(unsigned char enable)
+{
+ unsigned char regs[4];
+ if (enable) {
+ regs[0] = DINBC0;
+ regs[1] = DINBC2;
+ regs[2] = DINBC4;
+ regs[3] = DINBC6;
+ }
+ else
+ memset(regs, 0x8B, 4);
+
+ mpu_write_mem(CFG_LP_QUAT, 4, regs);
+
+ return mpu_reset_fifo();
+}
+
+/**
+ * @brief Generate 6-axis quaternions from the DMP.
+ * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
+ * exclusive.
+ * @param[in] enable 1 to enable 6-axis quaternion.
+ * @return 0 if successful.
+ */
+int dmp_enable_6x_lp_quat(unsigned char enable)
+{
+ unsigned char regs[4];
+ if (enable) {
+ regs[0] = DINA20;
+ regs[1] = DINA28;
+ regs[2] = DINA30;
+ regs[3] = DINA38;
+ } else
+ memset(regs, 0xA3, 4);
+
+ mpu_write_mem(CFG_8, 4, regs);
+
+ return mpu_reset_fifo();
+}
+
+/**
+ * @brief Decode the four-byte gesture data and execute any callbacks.
+ * @param[in] gesture Gesture data from DMP packet.
+ * @return 0 if successful.
+ */
+static int decode_gesture(unsigned char *gesture)
+{
+ unsigned char tap, android_orient;
+
+ android_orient = gesture[3] & 0xC0;
+ tap = 0x3F & gesture[3];
+
+ if (gesture[1] & INT_SRC_TAP) {
+ unsigned char direction, count;
+ direction = tap >> 3;
+ count = (tap % 8) + 1;
+ if (dmp.tap_cb)
+ dmp.tap_cb(direction, count);
+ }
+
+ if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
+ if (dmp.android_orient_cb)
+ dmp.android_orient_cb(android_orient >> 6);
+ }
+
+ return 0;
+}
+
+/**
+ * @brief Specify when a DMP interrupt should occur.
+ * A DMP interrupt can be configured to trigger on either of the two
+ * conditions below:
+ * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
+ * \n b. A tap event has been detected.
+ * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
+ * @return 0 if successful.
+ */
+int dmp_set_interrupt_mode(unsigned char mode)
+{
+ const unsigned char regs_continuous[11] =
+ {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
+ const unsigned char regs_gesture[11] =
+ {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
+
+ switch (mode) {
+ case DMP_INT_CONTINUOUS:
+ return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
+ (unsigned char*)regs_continuous);
+ case DMP_INT_GESTURE:
+ return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
+ (unsigned char*)regs_gesture);
+ default:
+ return -1;
+ }
+}
+
+/**
+ * @brief Get one packet from the FIFO.
+ * If @e sensors does not contain a particular sensor, disregard the data
+ * returned to that pointer.
+ * \n @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * \n INV_WXYZ_QUAT
+ * \n If the FIFO has no new data, @e sensors will be zero.
+ * \n If the FIFO is disabled, @e sensors will be zero and this function will
+ * return a non-zero error code.
+ * @param[out] gyro Gyro data in hardware units.
+ * @param[out] accel Accel data in hardware units.
+ * @param[out] quat 3-axis quaternion data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds.
+ * @param[out] sensors Mask of sensors read from FIFO.
+ * @param[out] more Number of remaining packets.
+ * @return 0 if successful.
+ */
+int dmp_read_fifo(short *gyro, short *accel, long *quat,
+ unsigned long *timestamp, short *sensors, unsigned char *more)
+{
+ unsigned char fifo_data[MAX_PACKET_LENGTH_2];
+ unsigned char ii = 0;
+
+ /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
+ * cache this value and save some cycles.
+ */
+ sensors[0] = 0;
+
+ /* Get a packet. */
+ if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
+ return -1;
+
+ /* Parse DMP packet. */
+ if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
+#ifdef FIFO_CORRUPTION_CHECK
+ long quat_q14[4], quat_mag_sq;
+#endif
+ quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
+ ((long)fifo_data[2] << 8) | fifo_data[3];
+ quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
+ ((long)fifo_data[6] << 8) | fifo_data[7];
+ quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
+ ((long)fifo_data[10] << 8) | fifo_data[11];
+ quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
+ ((long)fifo_data[14] << 8) | fifo_data[15];
+ ii += 16;
+#ifdef FIFO_CORRUPTION_CHECK
+ /* We can detect a corrupted FIFO by monitoring the quaternion data and
+ * ensuring that the magnitude is always normalized to one. This
+ * shouldn't happen in normal operation, but if an I2C error occurs,
+ * the FIFO reads might become misaligned.
+ *
+ * Let's start by scaling down the quaternion data to avoid long long
+ * math.
+ */
+ quat_q14[0] = quat[0] >> 16;
+ quat_q14[1] = quat[1] >> 16;
+ quat_q14[2] = quat[2] >> 16;
+ quat_q14[3] = quat[3] >> 16;
+ quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
+ quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
+ if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
+ (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
+ /* Quaternion is outside of the acceptable threshold. */
+ mpu_reset_fifo();
+ sensors[0] = 0;
+ return -1;
+ }
+ sensors[0] |= INV_WXYZ_QUAT;
+#endif
+ }
+
+ if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
+ accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
+ accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
+ accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
+ ii += 6;
+ sensors[0] |= INV_XYZ_ACCEL;
+ }
+
+ if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
+ gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
+ gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
+ gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
+ ii += 6;
+ sensors[0] |= INV_XYZ_GYRO;
+ }
+
+ /* Gesture data is at the end of the DMP packet. Parse it and call
+ * the gesture callbacks (if registered).
+ */
+ if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
+ decode_gesture(fifo_data + ii);
+
+ get_ms(timestamp);
+ return 0;
+}
+
+/**
+ * @brief Register a function to be executed on a tap event.
+ * The tap direction is represented by one of the following:
+ * \n TAP_X_UP
+ * \n TAP_X_DOWN
+ * \n TAP_Y_UP
+ * \n TAP_Y_DOWN
+ * \n TAP_Z_UP
+ * \n TAP_Z_DOWN
+ * @param[in] func Callback function.
+ * @return 0 if successful.
+ */
+int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
+{
+ dmp.tap_cb = func;
+ return 0;
+}
+
+/**
+ * @brief Register a function to be executed on a android orientation event.
+ * @param[in] func Callback function.
+ * @return 0 if successful.
+ */
+int dmp_register_android_orient_cb(void (*func)(unsigned char))
+{
+ dmp.android_orient_cb = func;
+ return 0;
+}