vs做网站通过e浏览器,推介网手机版,推广普通话内容100字,包装网站建设注#xff1a;文末附STM32驱动MPU6050代码工程链接#xff0c;需要的读者请自取。
一、MPU6050介绍 MPU6050是一款集成了三轴陀螺仪和三轴加速度计的传感器芯片#xff0c;由英国飞利浦半导体#xff08;现为恩智浦半导体#xff09;公司生产。它通过电子接口#xff08…注文末附STM32驱动MPU6050代码工程链接需要的读者请自取。
一、MPU6050介绍 MPU6050是一款集成了三轴陀螺仪和三轴加速度计的传感器芯片由英国飞利浦半导体现为恩智浦半导体公司生产。它通过电子接口如I2C或SPI与微控制器进行通信可用于测量物体的加速度和角速度广泛应用于无人机、机器人、游戏手柄、智能手机和其他电子设备中。 MPU6050内置的三轴陀螺仪用于测量物体的角速度或转动速率而三轴加速度计则用于测量物体的加速度或静止状态下的重力加速度。通过同时使用这两种传感器可以实现对物体姿态的精确测量例如倾斜角度、旋转速度等。 除了基本的数据输出功能MPU6050还具有一些高级功能比如嵌入式温度传感器、数字运动处理器DMP等。DMP可以处理传感器数据并通过内部FIFO缓冲区提供四元数等复杂的姿态解算数据从而减轻主控制器的负担简化软件开发流程。 总的来说MPU6050是一款功能强大、性能稳定的传感器模块适用于许多需要姿态检测和运动跟踪的应用场景。 二、加速度计 加速度计顾名思义,就是测量加速度的.那么,我们如何认识这个加速度呢?在此用一个盒子形 状的立方体来做模型认识加速度,如下,盒子内的图像。 如果我们把盒子形状的立方体 放在一个没引力场的地方,球会保持在盒子的中间.你可以想 象这个盒子是在外太空远离任何天体很难找到这样的地方,就想象飞船轨道围绕地球 飞一切都是在失重状态下。那么六个壁面感受到的压力都是0. 如果我们突然将立方体向左侧移动我们加快加速1G 9.8米/ S ^ 2皮球打在了 墙上X-。然后我们测量球适用于在X轴上的壁和输出-1g值的压力。如下图 请注意加速度计反应的加速向量与当前的受力方向是相反的.如上图所示,受力方向向 左,但是加速度的向量方向为右. 如果我们把这个小盒子拿来放在地球上,那么小球会落在Z-壁面上并会为1G的底壁施加一 个力在下面的图片所示 在这种情况下框不动但我们仍然可以得到Z轴的读数-1G。球在墙壁上的压力造成的引 力场。 到目前为止我们已经分析了单个轴加速度计的输出这时你会得到一个单轴加速度计。三轴加速度计的真正价值即是他们可以同时检测到所有三个轴的惯性力。让我们回到我 们的盒模型并让旋转45度在右边的框中。球会触及两面墙Z和X-在下面的图片所示 x 和 z 轴受到值的0.71是不是任意的? 它们实际上是一个近似SQRT1/2.要知道当盒子 只受重力场时x^2Y^2z^2 1g 这将变得更加清晰为大家介绍一下下一个样 子的加速度计。 在之前的盒子模型中我们有固定的引力场及旋转。在刚刚说明的两个例子中我们分析 了2个不同的输出框位置而力矢量保持不变。这更有利于了解加速度计如何与外部交互 并显示当前读数。 请看一下上面的模型这是一个新的模型代替刚刚的盒子立方体。试想一下在新模型中 的每个轴是垂直于盒子的壁面。向量R是加速度计测量从上面的例子或这两者的组合它 可以是引力场或惯性力的力矢量。接收时RyR Z与上的XYZ轴的R矢量投影。请注 意下面的关系式 R ^ 2 RX ^ 2 RY ^ 2 RZ ^ 2 公式1 请记住早一点我告诉你SQRT1/2的值不是随机的〜0.71。如果您将上述公式中回 顾我们的引力场为1 g后我们可以验证 1 ^ 2 -SQRT1/2^ 2 0 ^ 2 -SQRT1/2^ 2 简单地通过代以R 1时接收 SQRT1/2时Ry 0RZ SQRT1/2在方程 经过以上的理论分析我们越来越接近现实生活中的加速度计。 值RXRYRZ实际上是呈线性关系对应到真正的现实加速度计的x轴 y轴 z轴. 在此提出一个问题 加速度计如何将这些信息告诉我们 目前市面上的加速度计从输出上区分为两种一种是数字的另一种是模拟 的.miniAHRS 使用的是MPU6050三轴加速度计是I2C接口的数字传感器.通过特定的命令 可以配置加速度的量程,并将内部ADC的转换结果读出来. 现在我们有我们的加速度计的读数以LSB为单位的它仍然不是g9.8米/秒^ 2需要最后的转换我们要知道加速度计灵敏度通常表示为LSB /g。比方说当我们 选择2g的量程时对应的灵敏度 16384 LSB/ G 。为了得到最终的力值单位为g我们 用下面的公式 RX ADCRx /灵敏度 也就是说 当x轴的计数为ADCRx 时那么对应的加速度值就是 (ADCRx/16384)g.回到加速度向量模型将相关角度符号补上如下图 方向余弦 我们感兴趣的是向量R 和 X、Y、Z轴之间的角度将它们定义为 Axr Ayr Azr. 可以看到 由R 和Rx 组成的直角三角形 COSAxr RX / R 依此类推 COSAyr RY / R COSAzr RZ / R 可以得到 R SQRTRX ^ 2 RY ^ 2 RZ ^ 2 我们发现 当使用 arccos() 反余弦 : Axr arccos(RX / R) Ayr arccos(RY / R) Azr arccos(RZ / R) 已以通过很多公式解释加速度计模型。我们也会很快解释陀螺仪 以及如何用加速度计和陀 螺仪的数据进行整合以得到更精确的角度估计。 在这之前 我们先来看看更有用的公式 cosX cos(Axr) Rx / R cosY cos(Ayr) Ry / R cosZ cos(Azr) Rz / R 这三个公式通常被称为方向余弦。你可以轻松地验证 SQRTcosX ^ 2 COSY ^ 2 cosZ ^ 2 1 这个属性可以避免监视R矢量的模长度。很多时候我们只对惯性矢量方向感兴趣对 矢量进行规范化对简化程序运算很有意义。 重力向量 我们暂且从理论分析回到现实的传感器输出中,当水平放置MPU6050,只有Z轴感受到重力向 量它将输出1g。对应的ADC值就是16384 (2g的量程)。此时,R就是重力向量, Rx0. Ry0. Rz R 1g. 满足 R ^ 2 RX ^ 2 RY ^ 2 RZ ^ 2 得到重力向量与各个轴的夹角 Axr arccos(RX / R) 90度 Ayr arccos(RY / R) 90度 Azr arccos(RZ / R) 0 度 加速度计的标定 当MPU6050水平放置时理论上Z轴感受到重力 将读出16384。同时X 轴和Y轴的读数将是0. 可实际并不是这样的。这是由于每个芯片在制作时都不一样数据手册上的都是理论的 值真正的芯片在水平时Z轴可能并不是16384.我们需要找到当各个轴在0g重力时的计数 1g时的读数以及-1g时的读数得到一个补偿值在每次读取ADC结果后都进行补偿。这 个过程我们称之为标定。用数学公式表示为: ADCx K*Gx Offset ADCx 传感器输出 Gx真实的加速值Offset加速度为0g时传感器的输出K标度因数 三、陀螺仪 陀螺仪是测试角速度的传感器也有人把角速度说成角速率说的是一样的物理量。拿电 机做例子当我们说一个电机10转每秒。一转是360度,那么它的主轴在一秒内转过3600 度。也就是说这个电机在转动时的角速度是3600dps.dps 就是dergee per second 度每秒 (或者写成 deg/s). MPU6050 集成了三轴的陀螺仪.角速度全格感测范围为±250、±500、±1000与± 2000°/sec (dps).当选择量程为±250dps的时候将会得到分辩率为131LSB/(º/s).也就 是当载体在X轴转动1dps时ADC将输出131. 回到加速度向量模型将相关角度符号补上如下图 陀螺仪测量什么 MPU6050带有三个陀螺仪每个陀螺仪各自负责检测相应轴的转动速度也就是检测围绕 各个轴转动的速度。像三轴的陀螺仪将同时检测 X Y Z轴的旋转。 由上面这个模型图首先我们定义 Rxz - 是R向量在XZ平面上的投影 Ryz - 是R向量在XY平面上的投影 Rxz和Rz所形成的直角三角形利用勾股定理我们得到 Rxz ^ 2 RX ^ 2 RZ ^ 2和同样 Ryz ^ 2 RY ^ 2 RZ ^ 2 还要注意的是 R ^ 2 RXZ ^ 2 Ry^ 2这可以来自从公式1和上面的等式或它可以是来自于由R和Ryz R ^ 2 Ryz ^ 2 Rx^ 2 形成的直角三角 我们不会在本文中使用这些公式只是让读者认识到所有值之间的关系。 同时我们将定义Z轴和Rxz 、RyZ之间的夹角。 Axz - Rxz和Z轴间的夹角 Ayz - Ryz和Z轴间的夹角 现在看看,从这个模型中,陀螺仪测量什么? 上述的说明,已经知道陀螺仪测量角度的变化率.为了解释这一点让我们假设我们已经 测量围绕Y轴的旋转角这将是Axz角在时刻t0我们将其定义为Axz0接下来我们测 量这个角度是在稍后的时间t1是Axz1。变化率将被计算如下 RateAxz Axz1 - Axz0/t1 - t0 如果Axz单位是度并以秒为时间单位那么RateAxz将以deg / s表示。 MPU6050并不会以 deg / s 单位输出我们需要在读完后进行转换。先来看看各个量程对 应的灵敏度。 从ADC值到 dps 通过I2C接口读出来的转换结果ADC值并不是以度每秒为单位。一般按以下公式进行转 换 Anglerate ADCrate /灵敏度 以量程为±1000º/s为例说明如何转换。假设读取x轴的ADC值为200,从上表中得知在 ±1000º/s下的灵敏度为32.8LSB/(º/s) 。根据上面的公式 Anglerate 200/32.8 6.09756º/s 这就是说,MPU6050检测到模块正在以约6度每秒的速度绕X轴(或者叫在YZ平面上)旋转. ADC值并不都是正的,请注意,当出现负数时,意味着该设备从现有的正方向相反的方向旋转. 四、STM32驱动MPU6050
1、核心代码
1IIC通信
#include mpuiic.h
#include delay.h
/**************************************************************************作 者 小小扫地僧
**************************************************************************///MPU IIC 延时函数
void MPU_IIC_Delay(void)
{delay_us(2);
}//初始化IIC
void MPU_IIC_Init(void)
{ GPIO_InitTypeDef GPIO_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟GPIO_InitStructure.GPIO_Pin GPIO_Pin_6|GPIO_Pin_7; //端口配置GPIO_InitStructure.GPIO_Mode GPIO_Mode_Out_PP; //推挽输出GPIO_InitStructure.GPIO_Speed GPIO_Speed_50MHz; //50MGPIO_Init(GPIOB, GPIO_InitStructure); //根据设定参数初始化GPIOB
}
//产生IIC起始信号
void MPU_IIC_Start(void)
{MPU_SDA_OUT(); //sda线输出MPU_IIC_SDA1; MPU_IIC_SCL1;MPU_IIC_Delay();MPU_IIC_SDA0;//START:when CLK is high,DATA change form high to low MPU_IIC_Delay();MPU_IIC_SCL0;//钳住I2C总线准备发送或接收数据
}
//产生IIC停止信号
void MPU_IIC_Stop(void)
{MPU_SDA_OUT();//sda线输出MPU_IIC_SCL0;MPU_IIC_SDA0;//STOP:when CLK is high DATA change form low to highMPU_IIC_Delay();MPU_IIC_SCL1; MPU_IIC_SDA1;//发送I2C总线结束信号MPU_IIC_Delay();
}
//等待应答信号到来
//返回值1接收应答失败
// 0接收应答成功
u8 MPU_IIC_Wait_Ack(void)
{u8 ucErrTime0;MPU_SDA_IN(); //SDA设置为输入 MPU_IIC_SDA1;MPU_IIC_Delay(); MPU_IIC_SCL1;MPU_IIC_Delay(); while(MPU_READ_SDA){ucErrTime;if(ucErrTime250){MPU_IIC_Stop();return 1;}}MPU_IIC_SCL0;//时钟输出0 return 0;
}
//产生ACK应答
void MPU_IIC_Ack(void)
{MPU_IIC_SCL0;MPU_SDA_OUT();MPU_IIC_SDA0;MPU_IIC_Delay();MPU_IIC_SCL1;MPU_IIC_Delay();MPU_IIC_SCL0;
}
//不产生ACK应答
void MPU_IIC_NAck(void)
{MPU_IIC_SCL0;MPU_SDA_OUT();MPU_IIC_SDA1;MPU_IIC_Delay();MPU_IIC_SCL1;MPU_IIC_Delay();MPU_IIC_SCL0;
}
//IIC发送一个字节
//返回从机有无应答
//1有应答
//0无应答
void MPU_IIC_Send_Byte(u8 txd)
{ u8 t; MPU_SDA_OUT(); MPU_IIC_SCL0;//拉低时钟开始数据传输for(t0;t8;t){ MPU_IIC_SDA(txd0x80)7;txd1; MPU_IIC_SCL1;MPU_IIC_Delay(); MPU_IIC_SCL0; MPU_IIC_Delay();}
}
//读1个字节ack1时发送ACKack0发送nACK
u8 MPU_IIC_Read_Byte(unsigned char ack)
{unsigned char i,receive0;MPU_SDA_IN();//SDA设置为输入for(i0;i8;i ){MPU_IIC_SCL0; MPU_IIC_Delay();MPU_IIC_SCL1;receive1;if(MPU_READ_SDA)receive; MPU_IIC_Delay(); } if (!ack)MPU_IIC_NAck();//发送nACKelseMPU_IIC_Ack(); //发送ACK return receive;
}
2IIC通信头文件
#ifndef __MPUIIC_H
#define __MPUIIC_H
#include sys.h//IO方向设置
#define MPU_SDA_IN() {GPIOB-CRL0X0FFFFFFF;GPIOB-CRL|(uint32_t)828;}
#define MPU_SDA_OUT() {GPIOB-CRL0X0FFFFFFF;GPIOB-CRL|(uint32_t)328;}//IO操作函数
#define MPU_IIC_SCL PBout(6) //SCL
#define MPU_IIC_SDA PBout(7) //SDA
#define MPU_READ_SDA PBin(7) //SDA //IIC所有操作函数
void MPU_IIC_Delay(void); //MPU IIC延时函数
void MPU_IIC_Init(void); //初始化IIC的IO口
void MPU_IIC_Start(void); //发送IIC开始信号
void MPU_IIC_Stop(void); //发送IIC停止信号
void MPU_IIC_Send_Byte(u8 txd); //IIC发送一个字节
u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC读取一个字节
u8 MPU_IIC_Wait_Ack(void); //IIC等待ACK信号
void MPU_IIC_Ack(void); //IIC发送ACK信号
void MPU_IIC_NAck(void); //IIC不发送ACK信号void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr);
#endif 3mpu6050.c
#include mpu6050.h
#include sys.h
#include delay.h
#include usart.h
/**************************************************************************作 者 小小扫地僧
**************************************************************************/
//初始化MPU6050
//返回值:0,成功
//其他,错误代码
u8 MPU_Init(void)
{ u8 res; MPU_IIC_Init();//初始化IIC总线MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050delay_ms(100);MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050 MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dpsMPU_Set_Accel_Fsr(0); //加速度传感器,±2gMPU_Set_Rate(200); //设置采样率50HzMPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFOMPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效resMPU_Read_Byte(MPU_DEVICE_ID_REG); if(resMPU_ADDR)//器件ID正确{MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作MPU_Set_Rate(200); //设置采样率为50Hz}else return 1;return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{u8 data0;if(lpf188)data1;else if(lpf98)data2;else if(lpf42)data3;else if(lpf20)data4;else if(lpf10)data5;else data6; return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{u8 data;if(rate1000)rate1000;if(rate4)rate4;data1000/rate-1;dataMPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{u8 buf[2]; short raw;float temp;MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); raw((u16)buf[0]8)|buf[1]; temp36.53((double)raw)/340; return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{u8 buf[6],res; resMPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);if(res0){*gx((u16)buf[0]8)|buf[1]; *gy((u16)buf[2]8)|buf[3]; *gz((u16)buf[4]8)|buf[5];} return res;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{u8 buf[6],res; resMPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);if(res0){*ax((u16)buf[0]8)|buf[1]; *ay((u16)buf[2]8)|buf[3]; *az((u16)buf[4]8)|buf[5];} return res;
}
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{u8 i; MPU_IIC_Start(); MPU_IIC_Send_Byte((addr1)|0);//发送器件地址写命令 if(MPU_IIC_Wait_Ack()) //等待应答{MPU_IIC_Stop(); return 1; }MPU_IIC_Send_Byte(reg); //写寄存器地址MPU_IIC_Wait_Ack(); //等待应答for(i0;ilen;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,正常
//其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{ MPU_IIC_Start(); MPU_IIC_Send_Byte((addr1)|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((addr1)|1);//发送器件地址读命令 MPU_IIC_Wait_Ack(); //等待应答 while(len){if(len1)*bufMPU_IIC_Read_Byte(0);//读数据,发送nACK else *bufMPU_IIC_Read_Byte(1); //读数据,发送ACK len--;buf; } MPU_IIC_Stop(); //产生一个停止条件 return 0;
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data)
{ MPU_IIC_Start(); MPU_IIC_Send_Byte((MPU_ADDR1)|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:寄存器地址
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{u8 res;MPU_IIC_Start(); MPU_IIC_Send_Byte((MPU_ADDR1)|0);//发送器件地址写命令 MPU_IIC_Wait_Ack(); //等待应答 MPU_IIC_Send_Byte(reg); //写寄存器地址MPU_IIC_Wait_Ack(); //等待应答MPU_IIC_Start();MPU_IIC_Send_Byte((MPU_ADDR1)|1);//发送器件地址读命令 MPU_IIC_Wait_Ack(); //等待应答 resMPU_IIC_Read_Byte(0);//读取数据,发送nACK MPU_IIC_Stop(); //产生一个停止条件 return res;
}
4mpu6050.h
#ifndef __MPU6050_H
#define __MPU6050_H
#include mpuiic.h //#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs寄存器,可读取版本号,寄存器手册未提到
//#define MPU_PROD_ID_REG 0X0C //prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X
#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y
#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z
#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A
#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器
#define MPU_CFG_REG 0X1A //配置寄存器
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器
#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器#define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器
#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
#define MPU_INT_EN_REG 0X38 //中断使能寄存器
#define MPU_INT_STA_REG 0X3A //中断状态寄存器#define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器#define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器
#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器#define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器#define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器#define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器
#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器
#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1
#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器
#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR 0X68因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2)
//#define MPU_READ 0XD1
//#define MPU_WRITE 0XD0u8 MPU_Init(void); //初始化MPU6050
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
u8 MPU_Write_Byte(u8 reg,u8 data); //IIC写一个字节
u8 MPU_Read_Byte(u8 reg); //IIC读一个字节u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
u8 MPU_Set_Fifo(u8 sens);short MPU_Get_Temperature(void);
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);#endif5inv_mpu.c
/*$License:Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.See included License.txt for License information.$*/
/*** addtogroup DRIVERS Sensor Driver Layer* brief Hardware drivers to communicate with sensors via I2C.** {* file inv_mpu.c* brief An I2C-based driver for Invensense gyroscopes.* details This driver currently works for the following devices:* MPU6050* MPU6500* MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)* MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)*/
#include stdio.h
#include stdint.h
#include stdlib.h
#include string.h
#include math.h
#include inv_mpu.h
#include inv_mpu_dmp_motion_driver.h
#include mpu6050.h
#include delay.h
#include usart.h#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 log_i printf //打印信息
#define log_e printf //打印信息
/* labs is already defined by TIs toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((ab)?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 log_i MPL_LOGI
#define log_e MPL_LOGE
/* labs is already defined by TIs toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((ab)?a:b)
#elif defined EMPL_TARGET_UC3L0
/* Instead of using the standard TWI driver from the ASF library, were 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 log_i MPL_LOGI
#define log_e 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. */
#endifstatic 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_SECONDARYunsigned 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_SECONDARYunsigned 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 doesnt 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 MPU6050INV_LPA_1_25HZ,INV_LPA_5HZ,INV_LPA_20HZ,INV_LPA_40HZ
#elif defined MPU6500INV_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, //addr1024, //max_fifo118, //num_reg340, //temp_sens-521, //temp_offset256 //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 reg,
// .hw hw,
// .test test
//};
static struct gyro_state_s st{reg,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 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;elsetmp 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;elsetmp 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;log_i(%#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 {log_e(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) {log_e(Product ID read as 0 indicates device is either incompatible or an MPU3050.\n);return -1;} else if (rev 4) {log_i(Half sensitivity part found.\n);st.chip_cfg.accel_half 1;} elsest.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 {log_e(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, well 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_SECONDARYst.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_SECONDARYsetup_compass();if (mpu_set_compass_sample_rate(10))return -1;
#else/* Already disabled by setup_compass. */if (mpu_set_bypass(0))return -1;
#endifmpu_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 MPU6050tmp[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;elsetmp[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;
#endifst.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;elsedata 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;elsedata 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;elsedata 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;elsedata 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;elserate[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_SECONDARYmpu_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_SECONDARYrate[0] st.chip_cfg.compass_sample_rate;return 0;
#elserate[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_SECONDARYunsigned 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;
#elsereturn -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 gs.* 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 isnt 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)/* Youre not getting what you asked for. Some sensors are* asleep.*/result -1;elseresult 0;if (sensors || st.chip_cfg.lp_accel_mode)set_int_enable(1);elseset_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;elsepower_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_SECONDARYunsigned char user_ctrl;
#endifif (sensors INV_XYZ_GYRO)data INV_CLK_PLL;else if (sensors)data 0;elsedata 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_BYPASSif (sensors INV_XYZ_COMPASS)mpu_set_bypass(1);elsempu_set_bypass(0);
#elseif (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;elseuser_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
#endifst.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;
// log_i(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[index0] 8) | data[index1];accel[1] (data[index2] 8) | data[index3];accel[2] (data[index4] 8) | data[index5];sensors[0] | INV_XYZ_ACCEL;index 6;}if ((index ! packet_size) st.chip_cfg.fifo_enable INV_X_GYRO) {gyro[0] (data[index0] 8) | data[index1];sensors[0] | INV_X_GYRO;index 2;}if ((index ! packet_size) st.chip_cfg.fifo_enable INV_Y_GYRO) {gyro[1] (data[index0] 8) | data[index1];sensors[0] | INV_Y_GYRO;index 2;}if ((index ! packet_size) st.chip_cfg.fifo_enable INV_Z_GYRO) {gyro[2] (data[index0] 8) | data[index1];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;elsetmp ~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;elsetmp 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;elsetmp 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
#endifstatic 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;elsedata[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;elsedata[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_64BITgyro[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);/* Dont remove gravity! */accel[2] - 65536L;}
#elsegyro[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);/* Dont remove gravity! */if (accel[2] 0L)accel[2] - 65536L;elseaccel[2] 65536L;
#endifreturn 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 MPU6050const unsigned char tries 2;long gyro_st[3], accel_st[3];unsigned char accel_result, gyro_result;
#ifdef AK89xx_SECONDARYunsigned char compass_result;
#endifint ii;
#endifint 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;} elsedmp_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 MPU6050for (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.* Well 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_SECONDARYcompass_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(firmwareii, 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_SECONDARYunsigned 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. */log_e(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;
#endifreturn 0;
#elsereturn -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_SECONDARYunsigned char tmp[9];if (!(st.chip_cfg.sensors INV_XYZ_COMPASS))return -1;#ifdef AK89xx_BYPASSif (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, tmp8))return -1;
#elseif (i2c_read(st.hw-addr, st.reg-raw_compass, 8, tmp))return -1;
#endif#if defined AK8975_SECONDARY/* AK8975 doesnt 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 doesnt 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;
#endifdata[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;
#elsereturn -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_SECONDARYfsr[0] st.hw-compass_fsr;return 0;
#elsereturn -1;
#endif
}/*** brief Enters LP accel motion interrupt mode.* The behavior of this feature is very different between the MPU6050 and the* MPU6500. Each chips 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;elsethresh_hw thresh 5;
#elif defined MPU6500/* 1LSb 4mg. */if (thresh 1020)thresh_hw 255;else if (thresh 4)thresh_hw 1;elsethresh_hw thresh 2;
#endifif (!time)/* Minimum duration must be 1ms. */time 1;#if defined MPU6050if (lpa_freq 40)
#elif defined MPU6500if (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;} elsest.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 doesnt 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;elsedata[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;elsedata[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 {/* Dont 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 hasnt 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;
#endifst.chip_cfg.int_motion_only 0;return 0;
}
//
//添加的代码部分
//
//本程序只供学习使用未经作者许可不得用于其它任何用途
//ALIENTEK精英STM32开发板V3
//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,正常
// 其他,失败
u8 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 0x7) {/* Test passed. We can trust the gyro data here, so lets 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 MatrixXZY 001_010_000YXZ 010_000_001YZX 000_010_001ZXY 001_000_010ZYX 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;elseb 7; // errorreturn b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{}
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{u8 res0;MPU_IIC_Init(); //初始化IIC总线if(mpu_init()0) //初始化MPU6050{ resmpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器if(res)return 1; resmpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFOif(res)return 2; resmpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率if(res)return 3; resdmp_load_motion_driver_firmware(); //加载dmp固件if(res)return 4; resdmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向if(res)return 5; resdmp_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; resdmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)if(res)return 7; resrun_self_test(); //自检// if(res)return 8; resmpu_set_dmp_state(1); //使能DMPif(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,正常
// 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{float q01.0f,q10.0f,q20.0f,q30.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(sensorsINV_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*q0q1*q1-q2*q2-q3*q3) * 57.3; //yaw}else return 2;return 0;
}
(6)inv_mpu.h
/*$License:Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.See included License.txt for License information.$*/
/*** addtogroup DRIVERS Sensor Driver Layer* brief Hardware drivers to communicate with sensors via I2C.** {* file inv_mpu.h* brief An I2C-based driver for Invensense gyroscopes.* details This driver currently works for the following devices:* MPU6050* MPU6500* MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)* MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)*/#ifndef _INV_MPU_H_
#define _INV_MPU_H_
#include stm32f10x.h//定义输出速度
#define DEFAULT_MPU_HZ (100) //200Hz#define INV_X_GYRO (0x40)
#define INV_Y_GYRO (0x20)
#define INV_Z_GYRO (0x10)
#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)
#define INV_XYZ_ACCEL (0x08)
#define INV_XYZ_COMPASS (0x01)//移植官方MSP430 DMP驱动过来
struct int_param_s {
//#if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430void (*cb)(void);unsigned short pin;unsigned char lp_exit;unsigned char active_low;
//#elif defined EMPL_TARGET_UC3L0
// unsigned long pin;
// void (*cb)(volatile void*);
// void *arg;
//#endif
};#define MPU_INT_STATUS_DATA_READY (0x0001)
#define MPU_INT_STATUS_DMP (0x0002)
#define MPU_INT_STATUS_PLL_READY (0x0004)
#define MPU_INT_STATUS_I2C_MST (0x0008)
#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010)
#define MPU_INT_STATUS_ZMOT (0x0020)
#define MPU_INT_STATUS_MOT (0x0040)
#define MPU_INT_STATUS_FREE_FALL (0x0080)
#define MPU_INT_STATUS_DMP_0 (0x0100)
#define MPU_INT_STATUS_DMP_1 (0x0200)
#define MPU_INT_STATUS_DMP_2 (0x0400)
#define MPU_INT_STATUS_DMP_3 (0x0800)
#define MPU_INT_STATUS_DMP_4 (0x1000)
#define MPU_INT_STATUS_DMP_5 (0x2000)/* Set up APIs */
int mpu_init(void);
int mpu_init_slave(void);
int mpu_set_bypass(unsigned char bypass_on);/* Configuration APIs */
int mpu_lp_accel_mode(unsigned char rate);
int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,unsigned char lpa_freq);
int mpu_set_int_level(unsigned char active_low);
int mpu_set_int_latched(unsigned char enable);int mpu_set_dmp_state(unsigned char enable);
int mpu_get_dmp_state(unsigned char *enabled);int mpu_get_lpf(unsigned short *lpf);
int mpu_set_lpf(unsigned short lpf);int mpu_get_gyro_fsr(unsigned short *fsr);
int mpu_set_gyro_fsr(unsigned short fsr);int mpu_get_accel_fsr(unsigned char *fsr);
int mpu_set_accel_fsr(unsigned char fsr);int mpu_get_compass_fsr(unsigned short *fsr);int mpu_get_gyro_sens(float *sens);
int mpu_get_accel_sens(unsigned short *sens);int mpu_get_sample_rate(unsigned short *rate);
int mpu_set_sample_rate(unsigned short rate);
int mpu_get_compass_sample_rate(unsigned short *rate);
int mpu_set_compass_sample_rate(unsigned short rate);int mpu_get_fifo_config(unsigned char *sensors);
int mpu_configure_fifo(unsigned char sensors);int mpu_get_power_state(unsigned char *power_on);
int mpu_set_sensors(unsigned char sensors);int mpu_set_accel_bias(const long *accel_bias);/* Data getter/setter APIs */
int mpu_get_gyro_reg(short *data, unsigned long *timestamp);
int mpu_get_accel_reg(short *data, unsigned long *timestamp);
int mpu_get_compass_reg(short *data, unsigned long *timestamp);
int mpu_get_temperature(long *data, unsigned long *timestamp);int mpu_get_int_status(short *status);
int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more);
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more);
int mpu_reset_fifo(void);int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data);
int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data);
int mpu_load_firmware(unsigned short length, const unsigned char *firmware,unsigned short start_addr, unsigned short sample_rate);int mpu_reg_dump(void);
int mpu_read_reg(unsigned char reg, unsigned char *data);
int mpu_run_self_test(long *gyro, long *accel);
int mpu_register_tap_cb(void (*func)(unsigned char, unsigned char));
//自行添加的一些函数
void mget_ms(unsigned long *time);
unsigned short inv_row_2_scale(const signed char *row);
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
u8 run_self_test(void);
u8 mpu_dmp_init(void);
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw);#endif /* #ifndef _INV_MPU_H_ */
(7)inv_mpu_dmp_motion_driver.c
/*$License:Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.See included License.txt for License information.$*/
/*** addtogroup DRIVERS Sensor Driver Layer* brief Hardware drivers to communicate with sensors via I2C.** {* file inv_mpu_dmp_motion_driver.c* brief DMP image and interface functions.* details All functions are preceded by the dmp_ prefix to* differentiate among MPL and general driver function calls.*/
#include stdio.h
#include stdint.h
#include stdlib.h
#include string.h
#include math.h
#include inv_mpu.h
#include inv_mpu_dmp_motion_driver.h
#include dmpKey.h
#include dmpmap.h
#include usart.h
#include delay.h//定义目标板采用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 log_i printf
#define log_e 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 log_i MPL_LOGI
#define log_e MPL_LOGE#elif defined EMPL_TARGET_UC3L0
/* Instead of using the standard TWI driver from the ASF library, were 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 log_i MPL_LOGI
#define log_e 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 (22512)
#define D_0_24 (24512)#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 (32)#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 (1L24)
#define QUAT_MAG_SQ_NORMALIZED (1L28)
#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED QUAT_ERROR_THRESH)
#endifstruct 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_64BITgyro_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);
#elsegyro_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);
#endifregs[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_64BITaccel_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);
#elseaccel_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);
#endifregs[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, tmp2))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, tmp2))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, tmp2))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 \#defines 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;elsetmp[0] 0xD8;mpu_write_mem(CFG_27,1,tmp);if (mask DMP_FEATURE_GYRO_CAL)dmp_enable_gyro_cal(1);elsedmp_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;} elsetmp[0] 0xD8;mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);if (mask DMP_FEATURE_LP_QUAT)dmp_enable_lp_quat(1);elsedmp_enable_lp_quat(0);if (mask DMP_FEATURE_6X_LP_QUAT)dmp_enable_6x_lp_quat(1);elsedmp_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;}elsememset(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;} elsememset(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];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_CHECKlong quat_q14[4], quat_mag_sq;
#endifquat[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* shouldnt happen in normal operation, but if an I2C error occurs,* the FIFO reads might become misaligned.** Lets 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[ii0] 8) | fifo_data[ii1];accel[1] ((short)fifo_data[ii2] 8) | fifo_data[ii3];accel[2] ((short)fifo_data[ii4] 8) | fifo_data[ii5];ii 6;sensors[0] | INV_XYZ_ACCEL;}if (dmp.feature_mask DMP_FEATURE_SEND_ANY_GYRO) {gyro[0] ((short)fifo_data[ii0] 8) | fifo_data[ii1];gyro[1] ((short)fifo_data[ii2] 8) | fifo_data[ii3];gyro[2] ((short)fifo_data[ii4] 8) | fifo_data[ii5];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;
}/*** }*/
(8)inv_mpu_dmp_motion_driver.h
/*$License:Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.See included License.txt for License information.$*/
/*** addtogroup DRIVERS Sensor Driver Layer* brief Hardware drivers to communicate with sensors via I2C.** {* file inv_mpu_dmp_motion_driver.h* brief DMP image and interface functions.* details All functions are preceded by the dmp_ prefix to* differentiate among MPL and general driver function calls.*/
#ifndef _INV_MPU_DMP_MOTION_DRIVER_H_
#define _INV_MPU_DMP_MOTION_DRIVER_H_#define TAP_X (0x01)
#define TAP_Y (0x02)
#define TAP_Z (0x04)
#define TAP_XYZ (0x07)#define TAP_X_UP (0x01)
#define TAP_X_DOWN (0x02)
#define TAP_Y_UP (0x03)
#define TAP_Y_DOWN (0x04)
#define TAP_Z_UP (0x05)
#define TAP_Z_DOWN (0x06)#define ANDROID_ORIENT_PORTRAIT (0x00)
#define ANDROID_ORIENT_LANDSCAPE (0x01)
#define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02)
#define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03)#define DMP_INT_GESTURE (0x01)
#define DMP_INT_CONTINUOUS (0x02)#define DMP_FEATURE_TAP (0x001)
#define DMP_FEATURE_ANDROID_ORIENT (0x002)
#define DMP_FEATURE_LP_QUAT (0x004)
#define DMP_FEATURE_PEDOMETER (0x008)
#define DMP_FEATURE_6X_LP_QUAT (0x010)
#define DMP_FEATURE_GYRO_CAL (0x020)
#define DMP_FEATURE_SEND_RAW_ACCEL (0x040)
#define DMP_FEATURE_SEND_RAW_GYRO (0x080)
#define DMP_FEATURE_SEND_CAL_GYRO (0x100)#define INV_WXYZ_QUAT (0x100)/* Set up functions. */
int dmp_load_motion_driver_firmware(void);
int dmp_set_fifo_rate(unsigned short rate);
int dmp_get_fifo_rate(unsigned short *rate);
int dmp_enable_feature(unsigned short mask);
int dmp_get_enabled_features(unsigned short *mask);
int dmp_set_interrupt_mode(unsigned char mode);
int dmp_set_orientation(unsigned short orient);
int dmp_set_gyro_bias(long *bias);
int dmp_set_accel_bias(long *bias);/* Tap functions. */
int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char));
int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh);
int dmp_set_tap_axes(unsigned char axis);
int dmp_set_tap_count(unsigned char min_taps);
int dmp_set_tap_time(unsigned short time);
int dmp_set_tap_time_multi(unsigned short time);
int dmp_set_shake_reject_thresh(long sf, unsigned short thresh);
int dmp_set_shake_reject_time(unsigned short time);
int dmp_set_shake_reject_timeout(unsigned short time);/* Android orientation functions. */
int dmp_register_android_orient_cb(void (*func)(unsigned char));/* LP quaternion functions. */
int dmp_enable_lp_quat(unsigned char enable);
int dmp_enable_6x_lp_quat(unsigned char enable);/* Pedometer functions. */
int dmp_get_pedometer_step_count(unsigned long *count);
int dmp_set_pedometer_step_count(unsigned long count);
int dmp_get_pedometer_walk_time(unsigned long *time);
int dmp_set_pedometer_walk_time(unsigned long time);/* DMP gyro calibration functions. */
int dmp_enable_gyro_cal(unsigned char enable);/* Read function. This function should be called whenever the MPU interrupt is* detected.*/
int dmp_read_fifo(short *gyro, short *accel, long *quat,unsigned long *timestamp, short *sensors, unsigned char *more);#endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */
五、STM32驱动MPU6050开源工程
链接https://pan.baidu.com/s/14AMqZV39YNEWqqx0ESKJvw?pwdzxf1 提取码zxf1 --来自百度网盘超级会员V2的分享
链接https://pan.baidu.com/s/1b8bJiqHlFljnCREeMtJT6g?pwdzxf1 提取码zxf1 --来自百度网盘超级会员V2的分享
六、MPU6050资料
需要的读者可以下方留言或者私信我收到后会第一时间回复。
