liang brain
/
Ex_MPU6050_Driver_Balance_chenhuan
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
MPU6050_Driver_Balance
by 
Chen Huan
Diff: mpu6050.cpp
- Revision:
- 0:badebd32bd8b
--- /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;
+}