代码收藏家 STM32 HAL库MPU6050传感器原理深度解析：卡尔曼滤波与DMP数据处理及代码实践
目录
陀螺仪相关基础知识：
陀螺仪数据处理的三种方式：
加速度计，陀螺仪的工作原理：
陀螺仪在智能车中的应用：
MPU6050原理图和封装图：
IIC的原理：
硬件IIC和软件IIC的区别：
相同点
不同点
常规获取陀螺仪数据：
标准库：
MyI2C.c
MyI2C.h
MPU6050.c
MPU6050.h
MPU6050_Reg.h
HAL库：
硬件IIC
i2c.c:
i2c.h
mpu6050.c
软件IIC
GPIO.c
Myi2C.
DMP库处理陀螺仪数据：
dmpKey.h
dmpmap.h
inv_mpu.c
inv_mpu.h
inv_mpu_dmp_motion_driver.c
inv_mpu_dmp_motion_driver.h
mpu6050.c
mpu6050.h
main.c
软件模拟iic
IIC.h
IIC.c
智能车逐飞库：
陀螺仪相关基础知识：
陀螺仪是感受空间姿态的传感器，是控制小车平衡，判断和调节姿态的核心元件
六轴陀螺仪结合了三轴陀螺仪和三轴加速度计，其“六轴”分别为加速度xyz轴，角速度xyz轴，即既能感知角度变化，也能感知加速度变化，这六个量经过运算，可以返回三个姿态角：
俯仰角[ pitch ]（车头绕Y轴翘起角度），航向角[ roll ]（车身绕Z轴旋转角度），翻滚角[ yaw ]（侧轮绕X轴抬起角度）。
（九轴多了三轴磁力计，需要XCL和XDA，本文不讲述和研究相关内容）
陀螺仪数据处理的三种方式：
（没有移植）
1.互补滤波
嵌入式C语言使用低通滤波、高通滤波、互补滤波算法_互补滤波c程序-CSDN博客
2.卡尔曼滤波
卡尔曼滤波笔记+C代码实现_卡尔曼滤波c代码-CSDN博客
3.DMP
加速度计，陀螺仪的工作原理：
干活文章但较难：[陀螺仪底层到应用]——-学习草稿_陀螺仪数据3d数据怎么看-CSDN博客
陀螺仪、加速度计都是惯性测量元件的一种。而 MPU-6050 传感器的内部同时集成了陀螺仪和加速度传感器两种惯性测量元件。
这篇文章把原理讲述的非常详细。
加速度计、陀螺仪工作原理_加速度计和陀螺仪的工作原理-CSDN博客
陀螺仪在智能车中的应用：
参考文章：智能车入门——陀螺仪_智能车陀螺仪转向控制-CSDN博客
1.角速度计
角速度计返回的数据实际上是弧度制的角速度
旋转陀螺仪时，返回相应轴的角速度参考量。一般我们要对角速度积分得到角度，用起来直观方便。积分得到的角度是相对角度，开始积分时的姿态即为参考0角度。对于2ms的积分值为0.000124左右，实际使用时需要调整，将陀螺仪转动一周，计算得到的角度应为360。
/***放于定时器中2ms执行一次***/
fun()
{
Angle_yaw += GZ * 0.00012480f;
}
积分总会带来误差，随着时间不断增加。另外，陀螺仪还存在零点漂移现象，即车静置时会输出非0的角速度值， 每次上电时需要重新校准偏移值，或者定期校准，后续检测时减去偏移值即可消除此误差。
float GX_Zero = -11.5;//零点偏移值
float GY_Zero = 6;
float GZ_Zero = -1;
……
GX = gx0 – GX_Zero; //去零漂之后的陀螺仪采集值
GY = gy0 – GY_Zero;
GZ = gz0 – GZ_Zero;
（之后会处理零漂的代码）
2.加速度计
加速计返回的加速度并不是一般意义的加速度。 加速度计相当于一个重锤在中间的弹簧系统，四面八方有弹簧撑着它。平放在桌面时，有的弹簧被拉长，有的被压扁。变化时，不同的弹簧受到不同的压缩，从而侦测出不同方向的力。因此实际上静止不动时也会产生由于重力引起的“加速度”数值。注意这个“加速度”并不稳定，由于车模本身的摆动所产生的加速度会产生很大的干扰信号，因此不宜过于信赖这个数值。
只用加速度计就能得到车身的俯仰角，且是对地的绝对角度。摆动加速度计时，对应轴的重力加速度分量改变，利用反三角函数即可求出角度的变化。但因为加速度计的不稳定，叠加在重力测量信号上使输出信号不能很好的反映车模的倾
#include <math.h>
fun()
{
angle_ratio = (double)(AX / (AZ + 0.1));//+0.1防止分母为0
Angle_acc = (float)(atan(angle_ratio) * 57.29578049);
//加速度计得到的角度 57.3=180/3.14
}
三、更精确的数据处理
因为数据存在误差、不稳定、有零漂，因此需要对数据进行处理。
1.滤波
输入级使用算数平均滤波就可以，取十次求平均一般比较稳定。如果要使用加速度，可以再加一个系数比较小的低通滤波。
角速度记得去掉零点偏移值。
2.角度融合
角速度积分得到的角度比较稳，但会随时间慢慢漂移且只反映变化量；加速度取反三角函数得到的角度不稳，但跟随真实角度，且是对地的绝对角度。
平衡车整个过程都要用到角度这个变量，单纯角速度积分带来的漂移偏差不可忽视，而且参考点不好确定，所以需要用角度融合，将两者得出的角度进行滤波融合，取长补短，就能得到比较可靠的角度。
角度融合滤波的方法比较常用的有：互补滤波、二阶互补滤波、卡尔曼滤波、清华滤波、四元数滤波。
以下为互补滤波程序
/***互补滤波角度计算***/
void AngleGet(void
{
float dt = 0.0001249;//Gy 2ms时间积分系数
double angle_ratio;//加速度比值
Anglefiltering();//入口滤波，算数平均
/***以下为加速度计取反正切得到角度***/
angle_ratio=((double)ax)/(az+0.1);
Angle_acc=(float)atan(angle_ratio)*57.29578049;//加速度计得到的角
if(Angle_acc > 89)
Angle_acc = 89;
if(Angle_acc < -89)
Angle_acc = -89;
/***以下为角速度计积分，同融合加速度，得到角度***/
Gy = (float)(gy)
GY = Gy -GY_Zero; //去零漂之后的陀螺仪采集值
Angle = (float)(Angle-(float)(GY * dt));
Angle = Angle + (Angle_acc-Angle)*0.001;
//相当于Angle = Angle*(1-0.00105) + Angle_acc*0.001
}
最后两条语句是最关键的部分，简单解释一下：
当前值（陀螺仪）＝上次值＋角速度微分值
当前值（融合）＝当前值（陀螺仪）*(1-a) – 当前值（加速度)*a （a为权重）
陀螺仪积分得到的角度稳定，加速度得到的角度最接近真实值
以陀螺仪积分算出来的角度为主导，用加速度得到的角度作纠正，防止角度误差随时间不断累计
MPU6050原理图和封装图：
IIC的原理：
STM32通讯全面概述（持续更新未完成）_stm32 hal串口回调-CSDN博客
硬件IIC和软件IIC的区别：
软件IIC ACK是0应答，1为非应答
硬件IIC ACK是1应答，0为非应答
相同点
目的相同：无论是硬件IIC还是软件IIC，它们的主要目的都是为了在STM32微控制器上实现IIC（Inter-Integrated Circuit）通信，以连接和通信外部设备。
通信协议基础相同：两者都遵循IIC通信协议，包括起始信号、停止信号、数据位、应答位等基本要素，以确保与IIC从设备的兼容性和正确性。
不同点
- 实现方式：
- 硬件IIC：由STM32内部的硬件模块实现，使用CPU的时钟信号来控制数据传输和时序。这种方式无需软件过多干预，直接通过内部寄存器进行配置和控制。 HAL库和标准库都有写好的函数，按照库函数写发送函数，接收函数就行，但硬件会有制定的引脚
- 软件IIC：通过STM32的GPIO（通用输入输出）端口模拟IIC通信的时序和数据传输。这种方式需要软件编程来精确控制GPIO的状态，以模拟IIC通信的波形和时序。 只要是能设置为开漏输出模式，能有上拉电阻都行
- 通信速度和效率：
- 硬件IIC：由于由专门的硬件模块实现，其通信速度较快，可以达到几十MHz的速度，且对CPU的占用率较低，效率较高。
- 软件IIC：通信速度相对较慢，一般在几十kHz到几百kHz之间，且因为需要软件来模拟通信过程，所以可能会对CPU产生一定的占用，效率较低。
- 实现难度和灵活性：
- 硬件IIC：实现相对简单，无需编写复杂的代码，配置寄存器后即可使用。然而，其灵活性可能受限，因为受限于芯片上I2C外设的数量和性能参数。
- 软件IIC：实现相对复杂，需要深入了解IIC通信的协议和时序要求，并编写相应的软件代码来模拟IIC通信。但其灵活性较高，可以在STM32的任何GPIO上实现IIC通信，且可以根据需要调整通信速率和时序参数。
- 应用场景：
- 硬件IIC：适用于对通信速度和效率要求较高，且芯片上I2C外设数量充足的场景。
- 软件IIC：适用于硬件IIC无法满足需求，或者需要实现多路IIC通信、灵活的时序控制等场景。
综上所述，STM32中的硬件IIC和软件IIC在实现IIC通信方面具有不同的特点和优劣势。在选择使用哪种方式时，需要根据具体的应用场景和需求来综合考虑。
常规获取陀螺仪数据：
IIC和MPU6050从最底层开始搓代码讲解：[10-3] 软件I2C读写MPU6050_哔哩哔哩_bilibili
SDL是主机控制，SDA是从机控制
标准库：
MyI2C.c
#include "stm32f10x.h" // Device header
#include "Delay.h"
/*引脚配置层*/
/**
* 函 数：I2C写SCL引脚电平
* 参 数：BitValue 协议层传入的当前需要写入SCL的电平，范围0~1
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，当BitValue为0时，需要置SCL为低电平，当BitValue为1时，需要置SCL为高电平
*/
void MyI2C_W_SCL(uint8_t BitValue)
{
GPIO_WriteBit(GPIOB, GPIO_Pin_10, (BitAction)BitValue); //根据BitValue，设置SCL引脚的电平
Delay_us(10); //延时10us，防止时序频率超过要求
}
/**
* 函 数：I2C写SDA引脚电平
* 参 数：BitValue 协议层传入的当前需要写入SDA的电平，范围0~0xFF
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，当BitValue为0时，需要置SDA为低电平，当BitValue非0时，需要置SDA为高电平
*/
void MyI2C_W_SDA(uint8_t BitValue)
{
GPIO_WriteBit(GPIOB, GPIO_Pin_11, (BitAction)BitValue); //根据BitValue，设置SDA引脚的电平，BitValue要实现非0即1的特性
Delay_us(10); //延时10us，防止时序频率超过要求
}
/**
* 函 数：I2C读SDA引脚电平
* 参 数：无
* 返 回 值：协议层需要得到的当前SDA的电平，范围0~1
* 注意事项：此函数需要用户实现内容，当前SDA为低电平时，返回0，当前SDA为高电平时，返回1
*/
uint8_t MyI2C_R_SDA(void)
{
uint8_t BitValue;
BitValue = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11); //读取SDA电平
Delay_us(10); //延时10us，防止时序频率超过要求
return BitValue; //返回SDA电平
}
/**
* 函 数：I2C初始化
* 参 数：无
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，实现SCL和SDA引脚的初始化
*/
void MyI2C_Init(void)
{
/*开启时钟*/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //开启GPIOB的时钟
/*GPIO初始化*/
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure); //将PB10和PB11引脚初始化为开漏输出
/*设置默认电平*/
GPIO_SetBits(GPIOB, GPIO_Pin_10 | GPIO_Pin_11); //设置PB10和PB11引脚初始化后默认为高电平（释放总线状态）
}
/*协议层*/
/**
* 函 数：I2C起始
* 参 数：无
* 返 回 值：无
*/
void MyI2C_Start(void)
{
MyI2C_W_SDA(1); //释放SDA，确保SDA为高电平
MyI2C_W_SCL(1); //释放SCL，确保SCL为高电平
MyI2C_W_SDA(0); //在SCL高电平期间，拉低SDA，产生起始信号
MyI2C_W_SCL(0); //起始后把SCL也拉低，即为了占用总线，也为了方便总线时序的拼接
}
/**
* 函 数：I2C终止
* 参 数：无
* 返 回 值：无
*/
void MyI2C_Stop(void)
{
MyI2C_W_SDA(0); //拉低SDA，确保SDA为低电平
MyI2C_W_SCL(1); //释放SCL，使SCL呈现高电平
MyI2C_W_SDA(1); //在SCL高电平期间，释放SDA，产生终止信号
}
/**
* 函 数：I2C发送一个字节
* 参 数：Byte 要发送的一个字节数据，范围：0x00~0xFF
* 返 回 值：无
*/
void MyI2C_SendByte(uint8_t Byte)
{
uint8_t i;
for (i = 0; i < 8; i ++) //循环8次，主机依次发送数据的每一位
{
MyI2C_W_SDA(Byte & (0x80 >> i)); //使用掩码的方式取出Byte的指定一位数据并写入到SDA线
MyI2C_W_SCL(1); //释放SCL，从机在SCL高电平期间读取SDA
MyI2C_W_SCL(0); //拉低SCL，主机开始发送下一位数据
}
}
/**
* 函 数：I2C接收一个字节
* 参 数：无
* 返 回 值：接收到的一个字节数据，范围：0x00~0xFF
*/
uint8_t MyI2C_ReceiveByte(void)
{
uint8_t i, Byte = 0x00; //定义接收的数据，并赋初值0x00，此处必须赋初值0x00，后面会用到
MyI2C_W_SDA(1); //接收前，主机先确保释放SDA，避免干扰从机的数据发送
for (i = 0; i < 8; i ++) //循环8次，主机依次接收数据的每一位
{
MyI2C_W_SCL(1); //释放SCL，主机机在SCL高电平期间读取SDA
if (MyI2C_R_SDA() == 1){Byte |= (0x80 >> i);} //读取SDA数据，并存储到Byte变量
//当SDA为1时，置变量指定位为1，当SDA为0时，不做处理，指定位为默认的初值0
MyI2C_W_SCL(0); //拉低SCL，从机在SCL低电平期间写入SDA
}
return Byte; //返回接收到的一个字节数据
}
/**
* 函 数：I2C发送应答位
* 参 数：Byte 要发送的应答位，范围：0~1，0表示应答，1表示非应答
* 返 回 值：无
*/
void MyI2C_SendAck(uint8_t AckBit)
{
MyI2C_W_SDA(AckBit); //主机把应答位数据放到SDA线
MyI2C_W_SCL(1); //释放SCL，从机在SCL高电平期间，读取应答位
MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块
}
/**
* 函 数：I2C接收应答位
* 参 数：无
* 返 回 值：接收到的应答位，范围：0~1，0表示应答，1表示非应答
*/
uint8_t MyI2C_ReceiveAck(void)
{
uint8_t AckBit; //定义应答位变量
MyI2C_W_SDA(1); //接收前，主机先确保释放SDA，避免干扰从机的数据发送
MyI2C_W_SCL(1); //释放SCL，主机机在SCL高电平期间读取SDA
AckBit = MyI2C_R_SDA(); //将应答位存储到变量里
MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块
return AckBit; //返回定义应答位变量
}
MyI2C.h
#ifndef __MYI2C_H
#define __MYI2C_H
void MyI2C_Init(void);
void MyI2C_Start(void);
void MyI2C_Stop(void);
void MyI2C_SendByte(uint8_t Byte);
uint8_t MyI2C_ReceiveByte(void);
void MyI2C_SendAck(uint8_t AckBit);
uint8_t MyI2C_ReceiveAck(void);
#endif
MPU6050.c
加速度计量程及灵敏度
陀螺仪量程及灵敏度
换算公式
假设配置 AFS_SEL=0, 那么实际加速度和传感器读数的换算关系为：换算成 m / s 2 （ g 取 9.8 ）
假设配置
FS_SEL=1, 那么实际角速度和传感器读数的换算关系为：
引自：MPU6050配置及读数换算-CSDN博客
引自：正点原子STM32学习笔记——MPU6050介绍_mpu6050是什么-CSDN博客
里面有些数据类型可以修改一下
优化把选型做个数组
#include "stm32h7xx_hal.h" // Device header
#include "MyI2C.h"
#include "MPU6050_Reg.h"
#define MPU6050_ADDRESS 0xD0 //MPU6050的I2C从机地址
/**
* 函 数：MPU6050写寄存器
* 参 数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
* 参 数：Data 要写入寄存器的数据，范围：0x00~0xFF
* 返 回 值：无
*/
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
MyI2C_Start(); //I2C起始
MyI2C_SendByte(MPU6050_ADDRESS); //发送从机地址，读写位为0，表示即将写入
MyI2C_ReceiveAck(); //接收应答
MyI2C_SendByte(RegAddress); //发送寄存器地址
MyI2C_ReceiveAck(); //接收应答
MyI2C_SendByte(Data); //发送要写入寄存器的数据
MyI2C_ReceiveAck(); //接收应答
MyI2C_Stop(); //I2C终止
}
/**
* 函 数：MPU6050读寄存器
* 参 数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
* 返 回 值：读取寄存器的数据，范围：0x00~0xFF
*/
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
uint8_t Data;
MyI2C_Start(); //I2C起始
MyI2C_SendByte(MPU6050_ADDRESS); //发送从机地址，读写位为0，表示即将写入
MyI2C_ReceiveAck(); //接收应答
MyI2C_SendByte(RegAddress); //发送寄存器地址
MyI2C_ReceiveAck(); //接收应答
MyI2C_Start(); //I2C重复起始
MyI2C_SendByte(MPU6050_ADDRESS | 0x01); //发送从机地址，读写位为1，表示即将读取
MyI2C_ReceiveAck(); //接收应答
Data = MyI2C_ReceiveByte(); //接收指定寄存器的数据
MyI2C_SendAck(1); //发送应答，给从机非应答，终止从机的数据输出
MyI2C_Stop(); //I2C终止
return Data;
}
/**
* 函 数：MPU6050初始化
* 参 数：无
* 返 回 值：无
*/
void MPU6050_Init(void)
{
MyI2C_Init(); //先初始化底层的I2C
/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01); //电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00); //电源管理寄存器2，保持默认值0，所有轴均不待机
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); //采样率分频寄存器，配置采样率
MPU6050_WriteReg(MPU6050_CONFIG, 0x06); //配置寄存器，配置DLPF
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); //陀螺仪配置寄存器，选择满量程为±2000°/s
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18); //加速度计配置寄存器，选择满量程为±16g
}
/**
* 函 数：MPU6050获取ID号
* 参 数：无
* 返 回 值：MPU6050的ID号
*/
uint8_t MPU6050_GetID(void)
{
return MPU6050_ReadReg(MPU6050_WHO_AM_I); //返回WHO_AM_I寄存器的值
}
/**
* 函 数：MPU6050获取数据
* 参 数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
* 返 回 值：无
*/
void MPU6050_GetData_ACC(int16_t *AccX, int16_t *AccY, int16_t *AccZ)
{
uint8_t DataH, DataL; //定义数据高8位和低8位的变量
DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); //读取加速度计X轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); //读取加速度计X轴的低8位数据
*AccX = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); //读取加速度计Y轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); //读取加速度计Y轴的低8位数据
*AccY = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); //读取加速度计Z轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); //读取加速度计Z轴的低8位数据
*AccZ = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
}
/**
* 函 数：MPU6050获取数据角速度
* 参 数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
* 返 回 值：无
*/
void MPU6050_GetData_GYRO(int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
uint8_t DataH, DataL; //定义数据高8位和低8位的变量
DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); //读取陀螺仪X轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); //读取陀螺仪X轴的低8位数据
*GyroX = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); //读取陀螺仪Y轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); //读取陀螺仪Y轴的低8位数据
*GyroY = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); //读取陀螺仪Z轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); //读取陀螺仪Z轴的低8位数据
*GyroZ = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
}
/**
* 函 数：MPU6050实际数据角 转化为带物理单位的数据，单位为m/(s^2)
* 参 数：T_GyroX T_GyroY T_GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
* 返 回 值：无
*/
void MPU6050_transition_GYRO(int16_t *T_GyroX, int16_t *T_GyroY, int16_t *T_GyroZ)
{
int16_t GX,GY,GZ;
MPU6050_GetData_GYRO(&GX, &GY, &GZ);
*T_GyroX=GX/32768*16*9.8;
*T_GyroY=GY/32768*16*9.8;
*T_GyroY=GZ/32768*16*9.8;
}
/**
* 函 数：MPU6050实际加速度 转化为带物理单位的数据，单位：g(m/s^2)
* 参 数：T_AccX T_AccY T_AccZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
* 返 回 值：无
*/
void MPU6050_transition_ACC(int16_t *T_AccX, int16_t *T_AccY, int16_t *T_AccZ)
{
int16_t AX,AY,AZ;
MPU6050_GetData_ACC(&AX, &AY, &AZ);
*T_AccX=AX/16384;
*T_AccY=AY/16384;
*T_AccZ=AZ/16384;
}
MPU6050.h
#ifndef __MPU6050_H
#define __MPU6050_H
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data);
uint8_t MPU6050_ReadReg(uint8_t RegAddress);
void MPU6050_Init(void);
uint8_t MPU6050_GetID(void);
void MPU6050_GetData_GYRO(int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ);
void MPU6050_GetData_ACC(int16_t *AccX, int16_t *AccY, int16_t *AccZ);
void MPU6050_transition_ACC(int16_t *T_AccX, int16_t *T_AccY, int16_t *T_AccZ);
void MPU6050_transition_GYRO(int16_t *T_GyroX, int16_t *T_GyroY, int16_t *T_GyroZ);
#endifMPU6050_Reg.h
寄存器宏定义，每个寄存器地址在数据手册中都可见
#ifndef __MPU6050_REG_H
#define __MPU6050_REG_H
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
#endifHAL库：
没有单独说明列出来写的，就是按照标准库的也没问题
cubemx的配置
硬件IIC
不再需要Myi2C这个文档，在MPU6050初始化中直接使用HAL库自带的IIC传输函数就行
i2c.c:
/**
* 函 数：I2C初始化
* 参 数：无
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，实现SCL和SDA引脚的初始化
*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
I2C_HandleTypeDef hi2c1;
/* I2C1 init function */
void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 100000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(i2cHandle->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspInit 0 */
/* USER CODE END I2C1_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C1 GPIO Configuration
PB10 ------> I2C1_SCL
PB11 ------> I2C1_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* I2C1 clock enable */
__HAL_RCC_I2C1_CLK_ENABLE();
/* USER CODE BEGIN I2C1_MspInit 1 */
/* USER CODE END I2C1_MspInit 1 */
}
}
void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle)
{
if(i2cHandle->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspDeInit 0 */
/* USER CODE END I2C1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_I2C1_CLK_DISABLE();
/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB7 ------> I2C1_SDA
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10);
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_11);
/* USER CODE BEGIN I2C1_MspDeInit 1 */
/* USER CODE END I2C1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
i2c.h
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file i2c.h
* @brief This file contains all the function prototypes for
* the i2c.c file
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __I2C_H__
#define __I2C_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern I2C_HandleTypeDef hi2c1;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_I2C1_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __I2C_H__ */
mpu6050.c
#include "stm32h7xx_hal.h" // Device header
#include "MPU6050_Reg.h"
#include "main.h"
#include "MPU6050.h"
#define MPU6050_ADDRESS 0xD0 //MPU6050的I2C从机地址
/**
* 函数：MPU6050写寄存器
* 参数：RegAddress 寄存器地址
* 参数：Data 要写入寄存器的数据
* 返回值：无
*/
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
HAL_StatusTypeDef status;
// 发送设备地址 + 写操作 + 寄存器地址
status = HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, HAL_MAX_DELAY);
if (status != HAL_OK)
{
// 错误处理
// 可以添加代码来处理错误，比如重试或记录错误
}
}
/**
* 函数：MPU6050读寄存器
* 参数：RegAddress 寄存器地址
* 返回值：读取寄存器的数据
*/
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
uint8_t Data;
HAL_StatusTypeDef status;
// 发送设备地址 + 写操作 + 寄存器地址（为了告诉MPU6050我们要读哪个寄存器）
status = HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &RegAddress, 1, HAL_MAX_DELAY);
if (status != HAL_OK)
{
// 错误处理
return 0xFF; // 或者其他错误码
}
// 发送设备地址 + 读操作（从之前指定的寄存器地址读取数据）
status = HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, HAL_MAX_DELAY);
if (status != HAL_OK)
{
// 错误处理
return 0xFF; // 或者其他错误码
}
return Data;
}
/**
* 函 数：MPU6050初始化
* 参 数：无
* 返 回 值：无
*/
void MPU6050_Init(void)
{
/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01); //电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00); //电源管理寄存器2，保持默认值0，所有轴均不待机
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); //采样率分频寄存器，配置采样率
MPU6050_WriteReg(MPU6050_CONFIG, 0x06); //配置寄存器，配置DLPF
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); //陀螺仪配置寄存器，选择满量程为±2000°/s
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18); //加速度计配置寄存器，选择满量程为±16g
}
/**
* 函 数：MPU6050获取ID号
* 参 数：无
* 返 回 值：MPU6050的ID号
*/
uint8_t MPU6050_GetID(void)
{
return MPU6050_ReadReg(MPU6050_WHO_AM_I); //返回WHO_AM_I寄存器的值
}
/**
* 函 数：MPU6050获取数据
* 参 数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
* 返 回 值：无
*/
void MPU6050_GetData_ACC(int16_t *AccX, int16_t *AccY, int16_t *AccZ)
{
uint8_t DataH, DataL; //定义数据高8位和低8位的变量
DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); //读取加速度计X轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); //读取加速度计X轴的低8位数据
*AccX = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); //读取加速度计Y轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); //读取加速度计Y轴的低8位数据
*AccY = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); //读取加速度计Z轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); //读取加速度计Z轴的低8位数据
*AccZ = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
}
/**
* 函 数：MPU6050获取数据角速度
* 参 数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
* 返 回 值：无
*/
void MPU6050_GetData_GYRO(int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
uint8_t DataH, DataL; //定义数据高8位和低8位的变量
DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); //读取陀螺仪X轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); //读取陀螺仪X轴的低8位数据
*GyroX = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); //读取陀螺仪Y轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); //读取陀螺仪Y轴的低8位数据
*GyroY = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); //读取陀螺仪Z轴的高8位数据
DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); //读取陀螺仪Z轴的低8位数据
*GyroZ = (DataH << 8) | DataL; //数据拼接，通过输出参数返回
}
/**
* 函 数：MPU6050实际数据角 转化为带物理单位的数据，单位为m/(s^2)
* 参 数：T_GyroX T_GyroY T_GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
* 返 回 值：无
*/
void MPU6050_transition_GYRO(int16_t *T_GyroX, int16_t *T_GyroY, int16_t *T_GyroZ)
{
int16_t GX,GY,GZ;
MPU6050_GetData_GYRO(&GX, &GY, &GZ);
*T_GyroX=GX/32768*16*9.8;
*T_GyroY=GY/32768*16*9.8;
*T_GyroY=GZ/32768*16*9.8;
}
/**
* 函 数：MPU6050实际加速度 转化为带物理单位的数据，单位：g(m/s^2)
* 参 数：T_AccX T_AccY T_AccZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
* 返 回 值：无
*/
void MPU6050_transition_ACC(int16_t *T_AccX, int16_t *T_AccY, int16_t *T_AccZ)
{
int16_t AX,AY,AZ;
MPU6050_GetData_ACC(&AX, &AY, &AZ);
*T_AccX=AX/16384;
*T_AccY=AY/16384;
*T_AccY=AZ/16384;
} 软件IIC
cubemx在GPIO上面的配置：
GPIO.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file gpio.c
* @brief This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* @attention
*
* Copyright (c) 2024 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure GPIO */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10|GPIO_PIN_11, GPIO_PIN_RESET);
/*Configure GPIO pins : PA10 PA11 */
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
Myi2C.c
#include "stm32f1xx_hal.h" // Device header
#include "delay.h"
/*引脚配置层*/
/**
* 函 数：I2C写SCL引脚电平
* 参 数：BitValue 协议层传入的当前需要写入SCL的电平，范围0~1
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，当BitValue为0时，需要置SCL为低电平，当BitValue为1时，需要置SCL为高电平
*/
void MyI2C_W_SCL(uint8_t BitValue)
{
if(BitValue=0)
{
HAL_GPIO_WritePin(GPIOB, GPIO_Pin_10, GPIO_PIN_RESET);
}
else
{
HAL_GPIO_WritePin(GPIOB, GPIO_Pin_10, GPIO_PIN_SET);
}
delay_us(10); //延时10us，防止时序频率超过要求
}
/**
* 函 数：I2C写SDA引脚电平
* 参 数：BitValue 协议层传入的当前需要写入SDA的电平，范围0~0xFF
* 返 回 值：无
* 注意事项：此函数需要用户实现内容，当BitValue为0时，需要置SDA为低电平，当BitValue非0时，需要置SDA为高电平
*/
void MyI2C_W_SDA(uint8_t BitValue)
{
if(BitValue=0)
{
HAL_GPIO_WritePin(GPIOB, GPIO_Pin_11, GPIO_PIN_RESET);
}
else
{
HAL_GPIO_WritePin(GPIOB, GPIO_Pin_11, GPIO_PIN_SET);
}
delay_us(10); //延时10us，防止时序频率超过要求
}
/**
* 函 数：I2C读SDA引脚电平
* 参 数：无
* 返 回 值：协议层需要得到的当前SDA的电平，范围0~1
* 注意事项：此函数需要用户实现内容，当前SDA为低电平时，返回0，当前SDA为高电平时，返回1
*/
uint8_t MyI2C_R_SDA(void)
{
uint8_t BitValue;
BitValue = HAL_GPIO_ReadPin(GPIOB, GPIO_Pin_11); //读取SDA电平
delay_us(10); //延时10us，防止时序频率超过要求
return BitValue; //返回SDA电平
}
/*协议层*/
/**
* 函 数：I2C起始
* 参 数：无
* 返 回 值：无
*/
void MyI2C_Start(void)
{
MyI2C_W_SDA(1); //释放SDA，确保SDA为高电平
MyI2C_W_SCL(1); //释放SCL，确保SCL为高电平
MyI2C_W_SDA(0); //在SCL高电平期间，拉低SDA，产生起始信号
MyI2C_W_SCL(0); //起始后把SCL也拉低，即为了占用总线，也为了方便总线时序的拼接
}
/**
* 函 数：I2C终止
* 参 数：无
* 返 回 值：无
*/
void MyI2C_Stop(void)
{
MyI2C_W_SDA(0); //拉低SDA，确保SDA为低电平
MyI2C_W_SCL(1); //释放SCL，使SCL呈现高电平
MyI2C_W_SDA(1); //在SCL高电平期间，释放SDA，产生终止信号
}
/**
* 函 数：I2C发送一个字节
* 参 数：Byte 要发送的一个字节数据，范围：0x00~0xFF
* 返 回 值：无
*/
void MyI2C_SendByte(uint8_t Byte)
{
uint8_t i;
for (i = 0; i < 8; i ++) //循环8次，主机依次发送数据的每一位
{
MyI2C_W_SDA(Byte & (0x80 >> i)); //使用掩码的方式取出Byte的指定一位数据并写入到SDA线
MyI2C_W_SCL(1); //释放SCL，从机在SCL高电平期间读取SDA
MyI2C_W_SCL(0); //拉低SCL，主机开始发送下一位数据
}
}
/**
* 函 数：I2C接收一个字节
* 参 数：无
* 返 回 值：接收到的一个字节数据，范围：0x00~0xFF
*/
uint8_t MyI2C_ReceiveByte(void)
{
uint8_t i, Byte = 0x00; //定义接收的数据，并赋初值0x00，此处必须赋初值0x00，后面会用到
MyI2C_W_SDA(1); //接收前，主机先确保释放SDA，避免干扰从机的数据发送
for (i = 0; i < 8; i ++) //循环8次，主机依次接收数据的每一位
{
MyI2C_W_SCL(1); //释放SCL，主机机在SCL高电平期间读取SDA
if (MyI2C_R_SDA() == 1){Byte |= (0x80 >> i);} //读取SDA数据，并存储到Byte变量
//当SDA为1时，置变量指定位为1，当SDA为0时，不做处理，指定位为默认的初值0
MyI2C_W_SCL(0); //拉低SCL，从机在SCL低电平期间写入SDA
}
return Byte; //返回接收到的一个字节数据
}
/**
* 函 数：I2C发送应答位
* 参 数：Byte 要发送的应答位，范围：0~1，0表示应答，1表示非应答
* 返 回 值：无
*/
void MyI2C_SendAck(uint8_t AckBit)
{
MyI2C_W_SDA(AckBit); //主机把应答位数据放到SDA线
MyI2C_W_SCL(1); //释放SCL，从机在SCL高电平期间，读取应答位
MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块
}
/**
* 函 数：I2C接收应答位
* 参 数：无
* 返 回 值：接收到的应答位，范围：0~1，0表示应答，1表示非应答
*/
uint8_t MyI2C_ReceiveAck(void)
{
uint8_t AckBit; //定义应答位变量
MyI2C_W_SDA(1); //接收前，主机先确保释放SDA，避免干扰从机的数据发送
MyI2C_W_SCL(1); //释放SCL，主机机在SCL高电平期间读取SDA
AckBit = MyI2C_R_SDA(); //将应答位存储到变量里
MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块
return AckBit; //返回定义应答位变量
}
DMP库处理陀螺仪数据：
是为了获得欧拉角
STM32平台下官方DMP库6.12超详细移植教程_mpu motion driver下载-CSDN博客
里面的代码为标准库的代码，移植步骤与HAL库一样，因此本文不再赘述移植步骤，侧重于代码
没有备注/记录代码 (gitee.com) 全部代码下载（MPU6050是标准库，MPU6050_test是HAL库代码）
DMP库处理MPU6050得到的数据主要是指通过MPU6050的数字运动处理器（Digital Motion Processor, DMP）进行的数据处理结果。MPU6050是一款集成了3轴MEMS陀螺仪和3轴MEMS加速度计的6轴运动处理传感器，其内部的DMP模块能够对这些原始传感器数据进行复杂的处理，以提供更高级别的运动数据，如姿态信息（俯仰角、翻滚角、偏航角）等。
具体来说，DMP库处理MPU6050的数据包括以下几个步骤：
数据读取：首先，MPU6050通过其I2C或SPI接口读取陀螺仪和加速度计的原始数据。这些数据包括三轴的角速度（通常以度/秒为单位）和三轴的加速度（通常以重力加速度g为单位）。
数据处理：DMP模块接收到这些原始数据后，会运用内置的算法（如卡尔曼滤波算法）对数据进行处理。这些算法能够融合陀螺仪和加速度计的数据，以消除噪声和漂移，并计算出更准确的姿态信息。DMP模块还可以将处理后的数据直接输出为四元数或欧拉角等形式，方便后续使用。
数据输出：处理后的数据可以通过MPU6050的接口输出给微控制器或其他处理设备。这些数据可以用于各种应用，如姿态控制、运动跟踪、导航定位等。
dmpKey.h
/*
$License:
Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
$
*/
#ifndef DMPKEY_H__
#define DMPKEY_H__
#define KEY_CFG_25 (0)
#define KEY_CFG_24 (KEY_CFG_25 + 1)
#define KEY_CFG_26 (KEY_CFG_24 + 1)
#define KEY_CFG_27 (KEY_CFG_26 + 1)
#define KEY_CFG_21 (KEY_CFG_27 + 1)
#define KEY_CFG_20 (KEY_CFG_21 + 1)
#define KEY_CFG_TAP4 (KEY_CFG_20 + 1)
#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1)
#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1)
#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1)
#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1)
#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1)
#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1)
#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1)
#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1)
#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1)
#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1)
#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1)
#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1)
#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1)
#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1)
#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1)
#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1)
#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1)
#define KEY_FCFG_1 (KEY_CFG_23 + 1)
#define KEY_FCFG_3 (KEY_FCFG_1 + 1)
#define KEY_FCFG_2 (KEY_FCFG_3 + 1)
#define KEY_CFG_3D (KEY_FCFG_2 + 1)
#define KEY_CFG_3B (KEY_CFG_3D + 1)
#define KEY_CFG_3C (KEY_CFG_3B + 1)
#define KEY_FCFG_5 (KEY_CFG_3C + 1)
#define KEY_FCFG_4 (KEY_FCFG_5 + 1)
#define KEY_FCFG_7 (KEY_FCFG_4 + 1)
#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1)
#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1)
#define KEY_FCFG_6 (KEY_FCFG_AZ + 1)
#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1)
#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1)
#define KEY_CFG_14 (KEY_CFG_12 + 1)
#define KEY_CFG_15 (KEY_CFG_14 + 1)
#define KEY_CFG_16 (KEY_CFG_15 + 1)
#define KEY_CFG_18 (KEY_CFG_16 + 1)
#define KEY_CFG_6 (KEY_CFG_18 + 1)
#define KEY_CFG_7 (KEY_CFG_6 + 1)
#define KEY_CFG_4 (KEY_CFG_7 + 1)
#define KEY_CFG_5 (KEY_CFG_4 + 1)
#define KEY_CFG_2 (KEY_CFG_5 + 1)
#define KEY_CFG_3 (KEY_CFG_2 + 1)
#define KEY_CFG_1 (KEY_CFG_3 + 1)
#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1)
#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1)
#define KEY_CFG_9 (KEY_CFG_8 + 1)
#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1)
#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1)
#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1)
#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1)
#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1)
#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1)
#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1)
#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1)
#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1)
#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1)
/* MPU6050 keys */
#define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1)
#define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1)
#define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1)
#define KEY_D_0_22 (KEY_TEMPLABEL + 1)
#define KEY_D_0_24 (KEY_D_0_22 + 1)
#define KEY_D_0_36 (KEY_D_0_24 + 1)
#define KEY_D_0_52 (KEY_D_0_36 + 1)
#define KEY_D_0_96 (KEY_D_0_52 + 1)
#define KEY_D_0_104 (KEY_D_0_96 + 1)
#define KEY_D_0_108 (KEY_D_0_104 + 1)
#define KEY_D_0_163 (KEY_D_0_108 + 1)
#define KEY_D_0_188 (KEY_D_0_163 + 1)
#define KEY_D_0_192 (KEY_D_0_188 + 1)
#define KEY_D_0_224 (KEY_D_0_192 + 1)
#define KEY_D_0_228 (KEY_D_0_224 + 1)
#define KEY_D_0_232 (KEY_D_0_228 + 1)
#define KEY_D_0_236 (KEY_D_0_232 + 1)
#define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1)
#define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1)
#define KEY_D_1_4 (KEY_D_1_2 + 1)
#define KEY_D_1_8 (KEY_D_1_4 + 1)
#define KEY_D_1_10 (KEY_D_1_8 + 1)
#define KEY_D_1_24 (KEY_D_1_10 + 1)
#define KEY_D_1_28 (KEY_D_1_24 + 1)
#define KEY_D_1_36 (KEY_D_1_28 + 1)
#define KEY_D_1_40 (KEY_D_1_36 + 1)
#define KEY_D_1_44 (KEY_D_1_40 + 1)
#define KEY_D_1_72 (KEY_D_1_44 + 1)
#define KEY_D_1_74 (KEY_D_1_72 + 1)
#define KEY_D_1_79 (KEY_D_1_74 + 1)
#define KEY_D_1_88 (KEY_D_1_79 + 1)
#define KEY_D_1_90 (KEY_D_1_88 + 1)
#define KEY_D_1_92 (KEY_D_1_90 + 1)
#define KEY_D_1_96 (KEY_D_1_92 + 1)
#define KEY_D_1_98 (KEY_D_1_96 + 1)
#define KEY_D_1_100 (KEY_D_1_98 + 1)
#define KEY_D_1_106 (KEY_D_1_100 + 1)
#define KEY_D_1_108 (KEY_D_1_106 + 1)
#define KEY_D_1_112 (KEY_D_1_108 + 1)
#define KEY_D_1_128 (KEY_D_1_112 + 1)
#define KEY_D_1_152 (KEY_D_1_128 + 1)
#define KEY_D_1_160 (KEY_D_1_152 + 1)
#define KEY_D_1_168 (KEY_D_1_160 + 1)
#define KEY_D_1_175 (KEY_D_1_168 + 1)
#define KEY_D_1_176 (KEY_D_1_175 + 1)
#define KEY_D_1_178 (KEY_D_1_176 + 1)
#define KEY_D_1_179 (KEY_D_1_178 + 1)
#define KEY_D_1_218 (KEY_D_1_179 + 1)
#define KEY_D_1_232 (KEY_D_1_218 + 1)
#define KEY_D_1_236 (KEY_D_1_232 + 1)
#define KEY_D_1_240 (KEY_D_1_236 + 1)
#define KEY_D_1_244 (KEY_D_1_240 + 1)
#define KEY_D_1_250 (KEY_D_1_244 + 1)
#define KEY_D_1_252 (KEY_D_1_250 + 1)
#define KEY_D_2_12 (KEY_D_1_252 + 1)
#define KEY_D_2_96 (KEY_D_2_12 + 1)
#define KEY_D_2_108 (KEY_D_2_96 + 1)
#define KEY_D_2_208 (KEY_D_2_108 + 1)
#define KEY_FLICK_MSG (KEY_D_2_208 + 1)
#define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1)
#define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1)
#define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1)
#define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1)
#define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1)
#define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1)
#define KEY_D_2_244 (KEY_D_2_224 + 1)
#define KEY_D_2_248 (KEY_D_2_244 + 1)
#define KEY_D_2_252 (KEY_D_2_248 + 1)
#define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1)
#define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1)
#define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1)
#define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1)
#define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1)
#define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1)
#define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1)
#define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1)
#define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1)
#define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1)
#define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1)
#define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1)
#define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1)
#define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1)
#define KEY_D_CS_TAY (KEY_D_CS_TAX + 1)
#define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1)
#define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1)
#define KEY_D_CS_TGY (KEY_D_CS_TGX + 1)
#define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1)
#define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1)
#define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1)
#define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1)
#define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1)
/* Compass keys */
#define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1)
#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1)
#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1)
#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1)
#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1)
#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1)
#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1)
#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1)
#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1)
#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1)
#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1)
#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1)
/* Gesture Keys */
#define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1)
#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1)
#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1)
#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1)
#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1)
#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1)
#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1)
#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1)
#define KEY_D_ACT0 (KEY_DMP_ORIENT + 1)
#define KEY_D_ACSX (KEY_D_ACT0 + 1)
#define KEY_D_ACSY (KEY_D_ACSX + 1)
#define KEY_D_ACSZ (KEY_D_ACSY + 1)
#define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1)
#define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1)
#define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1)
#define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1)
#define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1)
#define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1)
#define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1)
#define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1)
#define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1)
#define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1)
#define KEY_X_GRT_Y (KEY_END_ORIENT + 1)
#define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1)
#define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1)
#define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1)
/* Authenticate Keys */
#define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1)
#define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1)
#define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1)
#define KEY_D_AUTH_B (KEY_D_AUTH_A + 1)
/* Pedometer standalone only keys */
#define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1)
#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1)
#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1)
#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1)
#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1)
#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1)
#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1)
#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1)
#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1)
#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1)
#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1)
#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1)
#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1)
#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1)
#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1)
#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1)
#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1)
#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1)
#define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1)
/*Host Based No Motion*/
#define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1)
/* EIS keys */
#define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1)
#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1)
#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1)
#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1)
#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1)
#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1)
#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1)
#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1)
#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1)
#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1)
#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1)
#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1)
#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1)
#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1)
#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1)
#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1)
#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1)
#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1)
#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1)
#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1)
#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1)
#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1)
#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1)
#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1)
#define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1)
#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1)
//#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1)
//#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1)
//#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1)
/* Stream keys */
#define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1)
#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1)
#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1)
#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1)
#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1)
#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1)
#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1)
#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1)
#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1)
#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1)
#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1)
#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1)
typedef struct {
unsigned short key;
unsigned short addr;
} tKeyLabel;
#define DINA0A 0x0a
#define DINA22 0x22
#define DINA42 0x42
#define DINA5A 0x5a
#define DINA06 0x06
#define DINA0E 0x0e
#define DINA16 0x16
#define DINA1E 0x1e
#define DINA26 0x26
#define DINA2E 0x2e
#define DINA36 0x36
#define DINA3E 0x3e
#define DINA46 0x46
#define DINA4E 0x4e
#define DINA56 0x56
#define DINA5E 0x5e
#define DINA66 0x66
#define DINA6E 0x6e
#define DINA76 0x76
#define DINA7E 0x7e
#define DINA00 0x00
#define DINA08 0x08
#define DINA10 0x10
#define DINA18 0x18
#define DINA20 0x20
#define DINA28 0x28
#define DINA30 0x30
#define DINA38 0x38
#define DINA40 0x40
#define DINA48 0x48
#define DINA50 0x50
#define DINA58 0x58
#define DINA60 0x60
#define DINA68 0x68
#define DINA70 0x70
#define DINA78 0x78
#define DINA04 0x04
#define DINA0C 0x0c
#define DINA14 0x14
#define DINA1C 0x1C
#define DINA24 0x24
#define DINA2C 0x2c
#define DINA34 0x34
#define DINA3C 0x3c
#define DINA44 0x44
#define DINA4C 0x4c
#define DINA54 0x54
#define DINA5C 0x5c
#define DINA64 0x64
#define DINA6C 0x6c
#define DINA74 0x74
#define DINA7C 0x7c
#define DINA01 0x01
#define DINA09 0x09
#define DINA11 0x11
#define DINA19 0x19
#define DINA21 0x21
#define DINA29 0x29
#define DINA31 0x31
#define DINA39 0x39
#define DINA41 0x41
#define DINA49 0x49
#define DINA51 0x51
#define DINA59 0x59
#define DINA61 0x61
#define DINA69 0x69
#define DINA71 0x71
#define DINA79 0x79
#define DINA25 0x25
#define DINA2D 0x2d
#define DINA35 0x35
#define DINA3D 0x3d
#define DINA4D 0x4d
#define DINA55 0x55
#define DINA5D 0x5D
#define DINA6D 0x6d
#define DINA75 0x75
#define DINA7D 0x7d
#define DINADC 0xdc
#define DINAF2 0xf2
#define DINAAB 0xab
#define DINAAA 0xaa
#define DINAF1 0xf1
#define DINADF 0xdf
#define DINADA 0xda
#define DINAB1 0xb1
#define DINAB9 0xb9
#define DINAF3 0xf3
#define DINA8B 0x8b
#define DINAA3 0xa3
#define DINA91 0x91
#define DINAB6 0xb6
#define DINAB4 0xb4
#define DINC00 0x00
#define DINC01 0x01
#define DINC02 0x02
#define DINC03 0x03
#define DINC08 0x08
#define DINC09 0x09
#define DINC0A 0x0a
#define DINC0B 0x0b
#define DINC10 0x10
#define DINC11 0x11
#define DINC12 0x12
#define DINC13 0x13
#define DINC18 0x18
#define DINC19 0x19
#define DINC1A 0x1a
#define DINC1B 0x1b
#define DINC20 0x20
#define DINC21 0x21
#define DINC22 0x22
#define DINC23 0x23
#define DINC28 0x28
#define DINC29 0x29
#define DINC2A 0x2a
#define DINC2B 0x2b
#define DINC30 0x30
#define DINC31 0x31
#define DINC32 0x32
#define DINC33 0x33
#define DINC38 0x38
#define DINC39 0x39
#define DINC3A 0x3a
#define DINC3B 0x3b
#define DINC40 0x40
#define DINC41 0x41
#define DINC42 0x42
#define DINC43 0x43
#define DINC48 0x48
#define DINC49 0x49
#define DINC4A 0x4a
#define DINC4B 0x4b
#define DINC50 0x50
#define DINC51 0x51
#define DINC52 0x52
#define DINC53 0x53
#define DINC58 0x58
#define DINC59 0x59
#define DINC5A 0x5a
#define DINC5B 0x5b
#define DINC60 0x60
#define DINC61 0x61
#define DINC62 0x62
#define DINC63 0x63
#define DINC68 0x68
#define DINC69 0x69
#define DINC6A 0x6a
#define DINC6B 0x6b
#define DINC70 0x70
#define DINC71 0x71
#define DINC72 0x72
#define DINC73 0x73
#define DINC78 0x78
#define DINC79 0x79
#define DINC7A 0x7a
#define DINC7B 0x7b
#define DIND40 0x40
#define DINA80 0x80
#define DINA90 0x90
#define DINAA0 0xa0
#define DINAC9 0xc9
#define DINACB 0xcb
#define DINACD 0xcd
#define DINACF 0xcf
#define DINAC8 0xc8
#define DINACA 0xca
#define DINACC 0xcc
#define DINACE 0xce
#define DINAD8 0xd8
#define DINADD 0xdd
#define DINAF8 0xf0
#define DINAFE 0xfe
#define DINBF8 0xf8
#define DINAC0 0xb0
#define DINAC1 0xb1
#define DINAC2 0xb4
#define DINAC3 0xb5
#define DINAC4 0xb8
#define DINAC5 0xb9
#define DINBC0 0xc0
#define DINBC2 0xc2
#define DINBC4 0xc4
#define DINBC6 0xc6
#endif // DMPKEY_H__
dmpmap.h
/*
$License:
Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
$
*/
#ifndef DMPMAP_H
#define DMPMAP_H
#ifdef __cplusplus
extern "C"
{
#endif
#define DMP_PTAT 0
#define DMP_XGYR 2
#define DMP_YGYR 4
#define DMP_ZGYR 6
#define DMP_XACC 8
#define DMP_YACC 10
#define DMP_ZACC 12
#define DMP_ADC1 14
#define DMP_ADC2 16
#define DMP_ADC3 18
#define DMP_BIASUNC 20
#define DMP_FIFORT 22
#define DMP_INVGSFH 24
#define DMP_INVGSFL 26
#define DMP_1H 28
#define DMP_1L 30
#define DMP_BLPFSTCH 32
#define DMP_BLPFSTCL 34
#define DMP_BLPFSXH 36
#define DMP_BLPFSXL 38
#define DMP_BLPFSYH 40
#define DMP_BLPFSYL 42
#define DMP_BLPFSZH 44
#define DMP_BLPFSZL 46
#define DMP_BLPFMTC 48
#define DMP_SMC 50
#define DMP_BLPFMXH 52
#define DMP_BLPFMXL 54
#define DMP_BLPFMYH 56
#define DMP_BLPFMYL 58
#define DMP_BLPFMZH 60
#define DMP_BLPFMZL 62
#define DMP_BLPFC 64
#define DMP_SMCTH 66
#define DMP_0H2 68
#define DMP_0L2 70
#define DMP_BERR2H 72
#define DMP_BERR2L 74
#define DMP_BERR2NH 76
#define DMP_SMCINC 78
#define DMP_ANGVBXH 80
#define DMP_ANGVBXL 82
#define DMP_ANGVBYH 84
#define DMP_ANGVBYL 86
#define DMP_ANGVBZH 88
#define DMP_ANGVBZL 90
#define DMP_BERR1H 92
#define DMP_BERR1L 94
#define DMP_ATCH 96
#define DMP_BIASUNCSF 98
#define DMP_ACT2H 100
#define DMP_ACT2L 102
#define DMP_GSFH 104
#define DMP_GSFL 106
#define DMP_GH 108
#define DMP_GL 110
#define DMP_0_5H 112
#define DMP_0_5L 114
#define DMP_0_0H 116
#define DMP_0_0L 118
#define DMP_1_0H 120
#define DMP_1_0L 122
#define DMP_1_5H 124
#define DMP_1_5L 126
#define DMP_TMP1AH 128
#define DMP_TMP1AL 130
#define DMP_TMP2AH 132
#define DMP_TMP2AL 134
#define DMP_TMP3AH 136
#define DMP_TMP3AL 138
#define DMP_TMP4AH 140
#define DMP_TMP4AL 142
#define DMP_XACCW 144
#define DMP_TMP5 146
#define DMP_XACCB 148
#define DMP_TMP8 150
#define DMP_YACCB 152
#define DMP_TMP9 154
#define DMP_ZACCB 156
#define DMP_TMP10 158
#define DMP_DZH 160
#define DMP_DZL 162
#define DMP_XGCH 164
#define DMP_XGCL 166
#define DMP_YGCH 168
#define DMP_YGCL 170
#define DMP_ZGCH 172
#define DMP_ZGCL 174
#define DMP_YACCW 176
#define DMP_TMP7 178
#define DMP_AFB1H 180
#define DMP_AFB1L 182
#define DMP_AFB2H 184
#define DMP_AFB2L 186
#define DMP_MAGFBH 188
#define DMP_MAGFBL 190
#define DMP_QT1H 192
#define DMP_QT1L 194
#define DMP_QT2H 196
#define DMP_QT2L 198
#define DMP_QT3H 200
#define DMP_QT3L 202
#define DMP_QT4H 204
#define DMP_QT4L 206
#define DMP_CTRL1H 208
#define DMP_CTRL1L 210
#define DMP_CTRL2H 212
#define DMP_CTRL2L 214
#define DMP_CTRL3H 216
#define DMP_CTRL3L 218
#define DMP_CTRL4H 220
#define DMP_CTRL4L 222
#define DMP_CTRLS1 224
#define DMP_CTRLSF1 226
#define DMP_CTRLS2 228
#define DMP_CTRLSF2 230
#define DMP_CTRLS3 232
#define DMP_CTRLSFNLL 234
#define DMP_CTRLS4 236
#define DMP_CTRLSFNL2 238
#define DMP_CTRLSFNL 240
#define DMP_TMP30 242
#define DMP_CTRLSFJT 244
#define DMP_TMP31 246
#define DMP_TMP11 248
#define DMP_CTRLSF2_2 250
#define DMP_TMP12 252
#define DMP_CTRLSF1_2 254
#define DMP_PREVPTAT 256
#define DMP_ACCZB 258
#define DMP_ACCXB 264
#define DMP_ACCYB 266
#define DMP_1HB 272
#define DMP_1LB 274
#define DMP_0H 276
#define DMP_0L 278
#define DMP_ASR22H 280
#define DMP_ASR22L 282
#define DMP_ASR6H 284
#define DMP_ASR6L 286
#define DMP_TMP13 288
#define DMP_TMP14 290
#define DMP_FINTXH 292
#define DMP_FINTXL 294
#define DMP_FINTYH 296
#define DMP_FINTYL 298
#define DMP_FINTZH 300
#define DMP_FINTZL 302
#define DMP_TMP1BH 304
#define DMP_TMP1BL 306
#define DMP_TMP2BH 308
#define DMP_TMP2BL 310
#define DMP_TMP3BH 312
#define DMP_TMP3BL 314
#define DMP_TMP4BH 316
#define DMP_TMP4BL 318
#define DMP_STXG 320
#define DMP_ZCTXG 322
#define DMP_STYG 324
#define DMP_ZCTYG 326
#define DMP_STZG 328
#define DMP_ZCTZG 330
#define DMP_CTRLSFJT2 332
#define DMP_CTRLSFJTCNT 334
#define DMP_PVXG 336
#define DMP_TMP15 338
#define DMP_PVYG 340
#define DMP_TMP16 342
#define DMP_PVZG 344
#define DMP_TMP17 346
#define DMP_MNMFLAGH 352
#define DMP_MNMFLAGL 354
#define DMP_MNMTMH 356
#define DMP_MNMTML 358
#define DMP_MNMTMTHRH 360
#define DMP_MNMTMTHRL 362
#define DMP_MNMTHRH 364
#define DMP_MNMTHRL 366
#define DMP_ACCQD4H 368
#define DMP_ACCQD4L 370
#define DMP_ACCQD5H 372
#define DMP_ACCQD5L 374
#define DMP_ACCQD6H 376
#define DMP_ACCQD6L 378
#define DMP_ACCQD7H 380
#define DMP_ACCQD7L 382
#define DMP_ACCQD0H 384
#define DMP_ACCQD0L 386
#define DMP_ACCQD1H 388
#define DMP_ACCQD1L 390
#define DMP_ACCQD2H 392
#define DMP_ACCQD2L 394
#define DMP_ACCQD3H 396
#define DMP_ACCQD3L 398
#define DMP_XN2H 400
#define DMP_XN2L 402
#define DMP_XN1H 404
#define DMP_XN1L 406
#define DMP_YN2H 408
#define DMP_YN2L 410
#define DMP_YN1H 412
#define DMP_YN1L 414
#define DMP_YH 416
#define DMP_YL 418
#define DMP_B0H 420
#define DMP_B0L 422
#define DMP_A1H 424
#define DMP_A1L 426
#define DMP_A2H 428
#define DMP_A2L 430
#define DMP_SEM1 432
#define DMP_FIFOCNT 434
#define DMP_SH_TH_X 436
#define DMP_PACKET 438
#define DMP_SH_TH_Y 440
#define DMP_FOOTER 442
#define DMP_SH_TH_Z 444
#define DMP_TEMP29 448
#define DMP_TEMP30 450
#define DMP_XACCB_PRE 452
#define DMP_XACCB_PREL 454
#define DMP_YACCB_PRE 456
#define DMP_YACCB_PREL 458
#define DMP_ZACCB_PRE 460
#define DMP_ZACCB_PREL 462
#define DMP_TMP22 464
#define DMP_TAP_TIMER 466
#define DMP_TAP_THX 468
#define DMP_TAP_THY 472
#define DMP_TAP_THZ 476
#define DMP_TAPW_MIN 478
#define DMP_TMP25 480
#define DMP_TMP26 482
#define DMP_TMP27 484
#define DMP_TMP28 486
#define DMP_ORIENT 488
#define DMP_THRSH 490
#define DMP_ENDIANH 492
#define DMP_ENDIANL 494
#define DMP_BLPFNMTCH 496
#define DMP_BLPFNMTCL 498
#define DMP_BLPFNMXH 500
#define DMP_BLPFNMXL 502
#define DMP_BLPFNMYH 504
#define DMP_BLPFNMYL 506
#define DMP_BLPFNMZH 508
#define DMP_BLPFNMZL 510
#ifdef __cplusplus
}
#endif
#endif // DMPMAP_H
inv_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"
#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 TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)
#elif defined EMPL_TARGET_MSP430
#include "msp430.h"
#include "msp430_i2c.h"
#include "msp430_clock.h"
#include "msp430_interrupt.h"
#include "log.h"
#define i2c_write msp430_i2c_write
#define i2c_read msp430_i2c_read
#define delay_ms msp430_delay_ms
#define get_ms msp430_get_clock_ms
static inline int reg_int_cb(struct int_param_s *int_param)
{
return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
int_param->active_low);
}
#define log_i MPL_LOGI
#define log_e MPL_LOGE
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)
#elif defined EMPL_TARGET_UC3L0
/* Instead of using the standard TWI driver from the ASF library, we're using
* a TWI driver that follows the slave address + register address convention.
*/
#include "twi.h"
#include "delay.h"
#include "sysclk.h"
#include "log.h"
#include "sensors_xplained.h"
#include "uc3l0_clock.h"
#define i2c_write(a, b, c, d) twi_write(a, b, d, c)
#define i2c_read(a, b, c, d) twi_read(a, b, d, c)
/* delay_ms is a function already defined in ASF. */
#define get_ms uc3l0_get_clock_ms
static inline int reg_int_cb(struct int_param_s *int_param)
{
sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
return 0;
}
#define 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." */
#endif
static int set_int_enable(unsigned char enable);
/* Hardware registers needed by driver. */
struct gyro_reg_s {
unsigned char who_am_i;
unsigned char rate_div;
unsigned char lpf;
unsigned char prod_id;
unsigned char user_ctrl;
unsigned char fifo_en;
unsigned char gyro_cfg;
unsigned char accel_cfg;
// unsigned char accel_cfg2;
// unsigned char lp_accel_odr;
unsigned char motion_thr;
unsigned char motion_dur;
unsigned char fifo_count_h;
unsigned char fifo_r_w;
unsigned char raw_gyro;
unsigned char raw_accel;
unsigned char temp;
unsigned char int_enable;
unsigned char dmp_int_status;
unsigned char int_status;
// unsigned char accel_intel;
unsigned char pwr_mgmt_1;
unsigned char pwr_mgmt_2;
unsigned char int_pin_cfg;
unsigned char mem_r_w;
unsigned char accel_offs;
unsigned char i2c_mst;
unsigned char bank_sel;
unsigned char mem_start_addr;
unsigned char prgm_start_h;
#if defined AK89xx_SECONDARY
unsigned char s0_addr;
unsigned char s0_reg;
unsigned char s0_ctrl;
unsigned char s1_addr;
unsigned char s1_reg;
unsigned char s1_ctrl;
unsigned char s4_ctrl;
unsigned char s0_do;
unsigned char s1_do;
unsigned char i2c_delay_ctrl;
unsigned char raw_compass;
/* The I2C_MST_VDDIO bit is in this register. */
unsigned char yg_offs_tc;
#endif
};
/* Information specific to a particular device. */
struct hw_s {
unsigned char addr;
unsigned short max_fifo;
unsigned char num_reg;
unsigned short temp_sens;
short temp_offset;
unsigned short bank_size;
#if defined AK89xx_SECONDARY
unsigned short compass_fsr;
#endif
};
/* When entering motion interrupt mode, the driver keeps track of the
* previous state so that it can be restored at a later time.
* TODO: This is tacky. Fix it.
*/
struct motion_int_cache_s {
unsigned short gyro_fsr;
unsigned char accel_fsr;
unsigned short lpf;
unsigned short sample_rate;
unsigned char sensors_on;
unsigned char fifo_sensors;
unsigned char dmp_on;
};
/* Cached chip configuration data.
* TODO: A lot of these can be handled with a bitmask.
*/
struct chip_cfg_s {
/* Matches gyro_cfg >> 3 & 0x03 */
unsigned char gyro_fsr;
/* Matches accel_cfg >> 3 & 0x03 */
unsigned char accel_fsr;
/* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
unsigned char sensors;
/* Matches config register. */
unsigned char lpf;
unsigned char clk_src;
/* Sample rate, NOT rate divider. */
unsigned short sample_rate;
/* Matches fifo_en register. */
unsigned char fifo_enable;
/* Matches int enable register. */
unsigned char int_enable;
/* 1 if devices on auxiliary I2C bus appear on the primary. */
unsigned char bypass_mode;
/* 1 if half-sensitivity.
* NOTE: This doesn't belong here, but everything else in hw_s is const,
* and this allows us to save some precious RAM.
*/
unsigned char accel_half;
/* 1 if device in low-power accel-only mode. */
unsigned char lp_accel_mode;
/* 1 if interrupts are only triggered on motion events. */
unsigned char int_motion_only;
struct motion_int_cache_s cache;
/* 1 for active low interrupts. */
unsigned char active_low_int;
/* 1 for latched interrupts. */
unsigned char latched_int;
/* 1 if DMP is enabled. */
unsigned char dmp_on;
/* Ensures that DMP will only be loaded once. */
unsigned char dmp_loaded;
/* Sampling rate used when DMP is enabled. */
unsigned short dmp_sample_rate;
#ifdef AK89xx_SECONDARY
/* Compass sample rate. */
unsigned short compass_sample_rate;
unsigned char compass_addr;
short mag_sens_adj[3];
#endif
};
/* Information for self-test. */
struct test_s {
unsigned long gyro_sens;
unsigned long accel_sens;
unsigned char reg_rate_div;
unsigned char reg_lpf;
unsigned char reg_gyro_fsr;
unsigned char reg_accel_fsr;
unsigned short wait_ms;
unsigned char packet_thresh;
float min_dps;
float max_dps;
float max_gyro_var;
float min_g;
float max_g;
float max_accel_var;
};
/* Gyro driver state variables. */
struct gyro_state_s {
const struct gyro_reg_s *reg;
const struct hw_s *hw;
struct chip_cfg_s chip_cfg;
const struct test_s *test;
};
/* Filter configurations. */
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
/* Full scale ranges. */
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
/* Full scale ranges. */
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
/* Clock sources. */
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
/* Low-power accel wakeup rates. */
enum lp_accel_rate_e {
#if defined MPU6050
INV_LPA_1_25HZ,
INV_LPA_5HZ,
INV_LPA_20HZ,
INV_LPA_40HZ
#elif defined MPU6500
INV_LPA_0_3125HZ,
INV_LPA_0_625HZ,
INV_LPA_1_25HZ,
INV_LPA_2_5HZ,
INV_LPA_5HZ,
INV_LPA_10HZ,
INV_LPA_20HZ,
INV_LPA_40HZ,
INV_LPA_80HZ,
INV_LPA_160HZ,
INV_LPA_320HZ,
INV_LPA_640HZ
#endif
};
#define BIT_I2C_MST_VDDIO (0x80)
#define BIT_FIFO_EN (0x40)
#define BIT_DMP_EN (0x80)
#define BIT_FIFO_RST (0x04)
#define BIT_DMP_RST (0x08)
#define BIT_FIFO_OVERFLOW (0x10)
#define BIT_DATA_RDY_EN (0x01)
#define BIT_DMP_INT_EN (0x02)
#define BIT_MOT_INT_EN (0x40)
#define BITS_FSR (0x18)
#define BITS_LPF (0x07)
#define BITS_HPF (0x07)
#define BITS_CLK (0x07)
#define BIT_FIFO_SIZE_1024 (0x40)
#define BIT_FIFO_SIZE_2048 (0x80)
#define BIT_FIFO_SIZE_4096 (0xC0)
#define BIT_RESET (0x80)
#define BIT_SLEEP (0x40)
#define BIT_S0_DELAY_EN (0x01)
#define BIT_S2_DELAY_EN (0x04)
#define BITS_SLAVE_LENGTH (0x0F)
#define BIT_SLAVE_BYTE_SW (0x40)
#define BIT_SLAVE_GROUP (0x10)
#define BIT_SLAVE_EN (0x80)
#define BIT_I2C_READ (0x80)
#define BITS_I2C_MASTER_DLY (0x1F)
#define BIT_AUX_IF_EN (0x20)
#define BIT_ACTL (0x80)
#define BIT_LATCH_EN (0x20)
#define BIT_ANY_RD_CLR (0x10)
#define BIT_BYPASS_EN (0x02)
#define BITS_WOM_EN (0xC0)
#define BIT_LPA_CYCLE (0x20)
#define BIT_STBY_XA (0x20)
#define BIT_STBY_YA (0x10)
#define BIT_STBY_ZA (0x08)
#define BIT_STBY_XG (0x04)
#define BIT_STBY_YG (0x02)
#define BIT_STBY_ZG (0x01)
#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
#if defined AK8975_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
#define AK89xx_FSR (9830)
#elif defined AK8963_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS (0x10)
#define AK89xx_FSR (4915)
#endif
#ifdef AK89xx_SECONDARY
#define AKM_REG_WHOAMI (0x00)
#define AKM_REG_ST1 (0x02)
#define AKM_REG_HXL (0x03)
#define AKM_REG_ST2 (0x09)
#define AKM_REG_CNTL (0x0A)
#define AKM_REG_ASTC (0x0C)
#define AKM_REG_ASAX (0x10)
#define AKM_REG_ASAY (0x11)
#define AKM_REG_ASAZ (0x12)
#define AKM_DATA_READY (0x01)
#define AKM_DATA_OVERRUN (0x02)
#define AKM_OVERFLOW (0x80)
#define AKM_DATA_ERROR (0x40)
#define AKM_BIT_SELF_TEST (0x40)
#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_WHOAMI (0x48)
#endif
#if defined MPU6050
//const struct gyro_reg_s reg = {
// .who_am_i = 0x75,
// .rate_div = 0x19,
// .lpf = 0x1A,
// .prod_id = 0x0C,
// .user_ctrl = 0x6A,
// .fifo_en = 0x23,
// .gyro_cfg = 0x1B,
// .accel_cfg = 0x1C,
// .motion_thr = 0x1F,
// .motion_dur = 0x20,
// .fifo_count_h = 0x72,
// .fifo_r_w = 0x74,
// .raw_gyro = 0x43,
// .raw_accel = 0x3B,
// .temp = 0x41,
// .int_enable = 0x38,
// .dmp_int_status = 0x39,
// .int_status = 0x3A,
// .pwr_mgmt_1 = 0x6B,
// .pwr_mgmt_2 = 0x6C,
// .int_pin_cfg = 0x37,
// .mem_r_w = 0x6F,
// .accel_offs = 0x06,
// .i2c_mst = 0x24,
// .bank_sel = 0x6D,
// .mem_start_addr = 0x6E,
// .prgm_start_h = 0x70
//#ifdef AK89xx_SECONDARY
// ,.raw_compass = 0x49,
// .yg_offs_tc = 0x01,
// .s0_addr = 0x25,
// .s0_reg = 0x26,
// .s0_ctrl = 0x27,
// .s1_addr = 0x28,
// .s1_reg = 0x29,
// .s1_ctrl = 0x2A,
// .s4_ctrl = 0x34,
// .s0_do = 0x63,
// .s1_do = 0x64,
// .i2c_delay_ctrl = 0x67
//#endif
//};
const struct gyro_reg_s reg = {
0x75, //who_am_i
0x19, //rate_div
0x1A, //lpf
0x0C, //prod_id
0x6A, //user_ctrl
0x23, //fifo_en
0x1B, //gyro_cfg
0x1C, //accel_cfg
0x1F, // motion_thr
0x20, // motion_dur
0x72, // fifo_count_h
0x74, // fifo_r_w
0x43, // raw_gyro
0x3B, // raw_accel
0x41, // temp
0x38, // int_enable
0x39, // dmp_int_status
0x3A, // int_status
0x6B, // pwr_mgmt_1
0x6C, // pwr_mgmt_2
0x37, // int_pin_cfg
0x6F, // mem_r_w
0x06, // accel_offs
0x24, // i2c_mst
0x6D, // bank_sel
0x6E, // mem_start_addr
0x70 // prgm_start_h
};
//const struct hw_s hw = {
// .addr = 0x68,
// .max_fifo = 1024,
// .num_reg = 118,
// .temp_sens = 340,
// .temp_offset = -521,
// .bank_size = 256
//#if defined AK89xx_SECONDARY
// ,.compass_fsr = AK89xx_FSR
//#endif
//};
const struct hw_s hw= {
0x68, //addr
1024, //max_fifo
118, //num_reg
340, //temp_sens
-521, //temp_offset
256 //bank_size
};
//const struct test_s test = {
// .gyro_sens = 32768/250,
// .accel_sens = 32768/16,
// .reg_rate_div = 0, /* 1kHz. */
// .reg_lpf = 1, /* 188Hz. */
// .reg_gyro_fsr = 0, /* 250dps. */
// .reg_accel_fsr = 0x18, /* 16g. */
// .wait_ms = 50,
// .packet_thresh = 5, /* 5% */
// .min_dps = 10.f,
// .max_dps = 105.f,
// .max_gyro_var = 0.14f,
// .min_g = 0.3f,
// .max_g = 0.95f,
// .max_accel_var = 0.14f
//};
const struct test_s test= {
32768/250, //gyro_sens
32768/16, // accel_sens
0, // reg_rate_div
1, // reg_lpf
0, // reg_gyro_fsr
0x18, // reg_accel_fsr
50, // wait_ms
5, // packet_thresh
10.0f, // min_dps
105.0f, // max_dps
0.14f, // max_gyro_var
0.3f, // min_g
0.95f, // max_g
0.14f // max_accel_var
};
//static struct gyro_state_s st = {
// .reg = ®,
// .hw = &hw,
// .test = &test
//};
static struct gyro_state_s st= {
®,
&hw,
{0},
&test
};
#elif defined MPU6500
const struct gyro_reg_s reg = {
.who_am_i = 0x75,
.rate_div = 0x19,
.lpf = 0x1A,
.prod_id = 0x0C,
.user_ctrl = 0x6A,
.fifo_en = 0x23,
.gyro_cfg = 0x1B,
.accel_cfg = 0x1C,
.accel_cfg2 = 0x1D,
.lp_accel_odr = 0x1E,
.motion_thr = 0x1F,
.motion_dur = 0x20,
.fifo_count_h = 0x72,
.fifo_r_w = 0x74,
.raw_gyro = 0x43,
.raw_accel = 0x3B,
.temp = 0x41,
.int_enable = 0x38,
.dmp_int_status = 0x39,
.int_status = 0x3A,
.accel_intel = 0x69,
.pwr_mgmt_1 = 0x6B,
.pwr_mgmt_2 = 0x6C,
.int_pin_cfg = 0x37,
.mem_r_w = 0x6F,
.accel_offs = 0x77,
.i2c_mst = 0x24,
.bank_sel = 0x6D,
.mem_start_addr = 0x6E,
.prgm_start_h = 0x70
#ifdef AK89xx_SECONDARY
,.raw_compass = 0x49,
.s0_addr = 0x25,
.s0_reg = 0x26,
.s0_ctrl = 0x27,
.s1_addr = 0x28,
.s1_reg = 0x29,
.s1_ctrl = 0x2A,
.s4_ctrl = 0x34,
.s0_do = 0x63,
.s1_do = 0x64,
.i2c_delay_ctrl = 0x67
#endif
};
const struct hw_s hw = {
.addr = 0x68,
.max_fifo = 1024,
.num_reg = 128,
.temp_sens = 321,
.temp_offset = 0,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/16,
.reg_rate_div = 0, /* 1kHz. */
.reg_lpf = 1, /* 188Hz. */
.reg_gyro_fsr = 0, /* 250dps. */
.reg_accel_fsr = 0x18, /* 16g. */
.wait_ms = 50,
.packet_thresh = 5, /* 5% */
.min_dps = 10.f,
.max_dps = 105.f,
.max_gyro_var = 0.14f,
.min_g = 0.3f,
.max_g = 0.95f,
.max_accel_var = 0.14f
};
static struct gyro_state_s st = {
.reg = ®,
.hw = &hw,
.test = &test
};
#endif
#define MAX_PACKET_LENGTH (12)
#ifdef AK89xx_SECONDARY
static int setup_compass(void);
#define MAX_COMPASS_SAMPLE_RATE (100)
#endif
/**
* @brief Enable/disable data ready interrupt.
* If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
* interrupt is used.
* @param[in] enable 1 to enable interrupt.
* @return 0 if successful.
*/
static int set_int_enable(unsigned char enable)
{
unsigned char tmp;
if (st.chip_cfg.dmp_on) {
if (enable)
tmp = BIT_DMP_INT_EN;
else
tmp = 0x00;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
return -1;
st.chip_cfg.int_enable = tmp;
} else {
if (!st.chip_cfg.sensors)
return -1;
if (enable && st.chip_cfg.int_enable)
return 0;
if (enable)
tmp = BIT_DATA_RDY_EN;
else
tmp = 0x00;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
return -1;
st.chip_cfg.int_enable = tmp;
}
return 0;
}
/**
* @brief Register dump for testing.
* @return 0 if successful.
*/
int mpu_reg_dump(void)
{
unsigned char ii;
unsigned char data;
for (ii = 0; ii < st.hw->num_reg; ii++) {
if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
continue;
if (i2c_read(st.hw->addr, ii, 1, &data))
return -1;
//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;
} else
st.chip_cfg.accel_half = 0;
}
#elif defined MPU6500
#define MPU6500_MEM_REV_ADDR (0x17)
if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
return -1;
if (rev == 0x1)
st.chip_cfg.accel_half = 0;
else {
//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, we'll use the last 1kB for the FIFO.
*/
data[0] = BIT_FIFO_SIZE_1024 | 0x8;
if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
return -1;
#endif
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.bypass_mode = 0xFF;
#ifdef AK89xx_SECONDARY
st.chip_cfg.compass_sample_rate = 0xFFFF;
#endif
/* mpu_set_sensors always preserves this setting. */
st.chip_cfg.clk_src = INV_CLK_PLL;
/* Handled in next call to mpu_set_bypass. */
st.chip_cfg.active_low_int = 1;
st.chip_cfg.latched_int = 0;
st.chip_cfg.int_motion_only = 0;
st.chip_cfg.lp_accel_mode = 0;
memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
st.chip_cfg.dmp_on = 0;
st.chip_cfg.dmp_loaded = 0;
st.chip_cfg.dmp_sample_rate = 0;
if (mpu_set_gyro_fsr(2000))
return -1;
if (mpu_set_accel_fsr(2))
return -1;
if (mpu_set_lpf(42))
return -1;
if (mpu_set_sample_rate(50))
return -1;
if (mpu_configure_fifo(0))
return -1;
// if (int_param)
// reg_int_cb(int_param);
#ifdef AK89xx_SECONDARY
setup_compass();
if (mpu_set_compass_sample_rate(10))
return -1;
#else
/* Already disabled by setup_compass. */
if (mpu_set_bypass(0))
return -1;
#endif
mpu_set_sensors(0);
return 0;
}
/**
* @brief Enter low-power accel-only mode.
* In low-power accel mode, the chip goes to sleep and only wakes up to sample
* the accelerometer at one of the following frequencies:
* \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
* \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
* \n If the requested rate is not one listed above, the device will be set to
* the next highest rate. Requesting a rate above the maximum supported
* frequency will result in an error.
* \n To select a fractional wake-up frequency, round down the value passed to
* @e rate.
* @param[in] rate Minimum sampling rate, or zero to disable LP
* accel mode.
* @return 0 if successful.
*/
int mpu_lp_accel_mode(unsigned char rate)
{
unsigned char tmp[2];
if (rate > 40)
return -1;
if (!rate) {
mpu_set_int_latched(0);
tmp[0] = 0;
tmp[1] = BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
return -1;
st.chip_cfg.lp_accel_mode = 0;
return 0;
}
/* For LP accel, we automatically configure the hardware to produce latched
* interrupts. In LP accel mode, the hardware cycles into sleep mode before
* it gets a chance to deassert the interrupt pin; therefore, we shift this
* responsibility over to the MCU.
*
* Any register read will clear the interrupt.
*/
mpu_set_int_latched(1);
#if defined MPU6050
tmp[0] = BIT_LPA_CYCLE;
if (rate == 1) {
tmp[1] = INV_LPA_1_25HZ;
mpu_set_lpf(5);
} else if (rate <= 5) {
tmp[1] = INV_LPA_5HZ;
mpu_set_lpf(5);
} else if (rate <= 20) {
tmp[1] = INV_LPA_20HZ;
mpu_set_lpf(10);
} else {
tmp[1] = INV_LPA_40HZ;
mpu_set_lpf(20);
}
tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
return -1;
#elif defined MPU6500
/* Set wake frequency. */
if (rate == 1)
tmp[0] = INV_LPA_1_25HZ;
else if (rate == 2)
tmp[0] = INV_LPA_2_5HZ;
else if (rate <= 5)
tmp[0] = INV_LPA_5HZ;
else if (rate <= 10)
tmp[0] = INV_LPA_10HZ;
else if (rate <= 20)
tmp[0] = INV_LPA_20HZ;
else if (rate <= 40)
tmp[0] = INV_LPA_40HZ;
else if (rate <= 80)
tmp[0] = INV_LPA_80HZ;
else if (rate <= 160)
tmp[0] = INV_LPA_160HZ;
else if (rate <= 320)
tmp[0] = INV_LPA_320HZ;
else
tmp[0] = INV_LPA_640HZ;
if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
return -1;
tmp[0] = BIT_LPA_CYCLE;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
return -1;
#endif
st.chip_cfg.sensors = INV_XYZ_ACCEL;
st.chip_cfg.clk_src = 0;
st.chip_cfg.lp_accel_mode = 1;
mpu_configure_fifo(0);
return 0;
}
/**
* @brief Read raw gyro data directly from the registers.
* @param[out] data Raw data in hardware units.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
{
unsigned char tmp[6];
if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
return -1;
if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
return -1;
data[0] = (tmp[0] << 8) | tmp[1];
data[1] = (tmp[2] << 8) | tmp[3];
data[2] = (tmp[4] << 8) | tmp[5];
if (timestamp)
get_ms(timestamp);
return 0;
}
/**
* @brief Read raw accel data directly from the registers.
* @param[out] data Raw data in hardware units.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_accel_reg(short *data, unsigned long *timestamp)
{
unsigned char tmp[6];
if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
return -1;
if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
return -1;
data[0] = (tmp[0] << 8) | tmp[1];
data[1] = (tmp[2] << 8) | tmp[3];
data[2] = (tmp[4] << 8) | tmp[5];
if (timestamp)
get_ms(timestamp);
return 0;
}
/**
* @brief Read temperature data directly from the registers.
* @param[out] data Data in q16 format.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_temperature(long *data, unsigned long *timestamp)
{
unsigned char tmp[2];
short raw;
if (!(st.chip_cfg.sensors))
return -1;
if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
return -1;
raw = (tmp[0] << 8) | tmp[1];
if (timestamp)
get_ms(timestamp);
data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
return 0;
}
/**
* @brief Push biases to the accel bias registers.
* This function expects biases relative to the current sensor output, and
* these biases will be added to the factory-supplied values.
* @param[in] accel_bias New biases.
* @return 0 if successful.
*/
int mpu_set_accel_bias(const long *accel_bias)
{
unsigned char data[6];
short accel_hw[3];
short got_accel[3];
short fg[3];
if (!accel_bias)
return -1;
if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
return 0;
if (i2c_read(st.hw->addr, 3, 3, data))
return -1;
fg[0] = ((data[0] >> 4) + 8) & 0xf;
fg[1] = ((data[1] >> 4) + 8) & 0xf;
fg[2] = ((data[2] >> 4) + 8) & 0xf;
accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
if (i2c_read(st.hw->addr, 0x06, 6, data))
return -1;
got_accel[0] = ((short)data[0] << 8) | data[1];
got_accel[1] = ((short)data[2] << 8) | data[3];
got_accel[2] = ((short)data[4] << 8) | data[5];
accel_hw[0] += got_accel[0];
accel_hw[1] += got_accel[1];
accel_hw[2] += got_accel[2];
data[0] = (accel_hw[0] >> 8) & 0xff;
data[1] = (accel_hw[0]) & 0xff;
data[2] = (accel_hw[1] >> 8) & 0xff;
data[3] = (accel_hw[1]) & 0xff;
data[4] = (accel_hw[2] >> 8) & 0xff;
data[5] = (accel_hw[2]) & 0xff;
if (i2c_write(st.hw->addr, 0x06, 6, data))
return -1;
return 0;
}
/**
* @brief Reset FIFO read/write pointers.
* @return 0 if successful.
*/
int mpu_reset_fifo(void)
{
unsigned char data;
if (!(st.chip_cfg.sensors))
return -1;
data = 0;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
return -1;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
return -1;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
return -1;
if (st.chip_cfg.dmp_on) {
data = BIT_FIFO_RST | BIT_DMP_RST;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
return -1;
delay_ms(50);
data = BIT_DMP_EN | BIT_FIFO_EN;
if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
data |= BIT_AUX_IF_EN;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
return -1;
if (st.chip_cfg.int_enable)
data = BIT_DMP_INT_EN;
else
data = 0;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
return -1;
data = 0;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
return -1;
} else {
data = BIT_FIFO_RST;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
return -1;
if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
data = BIT_FIFO_EN;
else
data = BIT_FIFO_EN | BIT_AUX_IF_EN;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
return -1;
delay_ms(50);
if (st.chip_cfg.int_enable)
data = BIT_DATA_RDY_EN;
else
data = 0;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
return -1;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
return -1;
}
return 0;
}
/**
* @brief Get the gyro full-scale range.
* @param[out] fsr Current full-scale range.
* @return 0 if successful.
*/
int mpu_get_gyro_fsr(unsigned short *fsr)
{
switch (st.chip_cfg.gyro_fsr) {
case INV_FSR_250DPS:
fsr[0] = 250;
break;
case INV_FSR_500DPS:
fsr[0] = 500;
break;
case INV_FSR_1000DPS:
fsr[0] = 1000;
break;
case INV_FSR_2000DPS:
fsr[0] = 2000;
break;
default:
fsr[0] = 0;
break;
}
return 0;
}
/**
* @brief Set the gyro full-scale range.
* @param[in] fsr Desired full-scale range.
* @return 0 if successful.
*/
int mpu_set_gyro_fsr(unsigned short fsr)
{
unsigned char data;
if (!(st.chip_cfg.sensors))
return -1;
switch (fsr) {
case 250:
data = INV_FSR_250DPS << 3;
break;
case 500:
data = INV_FSR_500DPS << 3;
break;
case 1000:
data = INV_FSR_1000DPS << 3;
break;
case 2000:
data = INV_FSR_2000DPS << 3;
break;
default:
return -1;
}
if (st.chip_cfg.gyro_fsr == (data >> 3))
return 0;
if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
return -1;
st.chip_cfg.gyro_fsr = data >> 3;
return 0;
}
/**
* @brief Get the accel full-scale range.
* @param[out] fsr Current full-scale range.
* @return 0 if successful.
*/
int mpu_get_accel_fsr(unsigned char *fsr)
{
switch (st.chip_cfg.accel_fsr) {
case INV_FSR_2G:
fsr[0] = 2;
break;
case INV_FSR_4G:
fsr[0] = 4;
break;
case INV_FSR_8G:
fsr[0] = 8;
break;
case INV_FSR_16G:
fsr[0] = 16;
break;
default:
return -1;
}
if (st.chip_cfg.accel_half)
fsr[0] <<= 1;
return 0;
}
/**
* @brief Set the accel full-scale range.
* @param[in] fsr Desired full-scale range.
* @return 0 if successful.
*/
int mpu_set_accel_fsr(unsigned char fsr)
{
unsigned char data;
if (!(st.chip_cfg.sensors))
return -1;
switch (fsr) {
case 2:
data = INV_FSR_2G << 3;
break;
case 4:
data = INV_FSR_4G << 3;
break;
case 8:
data = INV_FSR_8G << 3;
break;
case 16:
data = INV_FSR_16G << 3;
break;
default:
return -1;
}
if (st.chip_cfg.accel_fsr == (data >> 3))
return 0;
if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
return -1;
st.chip_cfg.accel_fsr = data >> 3;
return 0;
}
/**
* @brief Get the current DLPF setting.
* @param[out] lpf Current LPF setting.
* 0 if successful.
*/
int mpu_get_lpf(unsigned short *lpf)
{
switch (st.chip_cfg.lpf) {
case INV_FILTER_188HZ:
lpf[0] = 188;
break;
case INV_FILTER_98HZ:
lpf[0] = 98;
break;
case INV_FILTER_42HZ:
lpf[0] = 42;
break;
case INV_FILTER_20HZ:
lpf[0] = 20;
break;
case INV_FILTER_10HZ:
lpf[0] = 10;
break;
case INV_FILTER_5HZ:
lpf[0] = 5;
break;
case INV_FILTER_256HZ_NOLPF2:
case INV_FILTER_2100HZ_NOLPF:
default:
lpf[0] = 0;
break;
}
return 0;
}
/**
* @brief Set digital low pass filter.
* The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
* @param[in] lpf Desired LPF setting.
* @return 0 if successful.
*/
int mpu_set_lpf(unsigned short lpf)
{
unsigned char data;
if (!(st.chip_cfg.sensors))
return -1;
if (lpf >= 188)
data = INV_FILTER_188HZ;
else if (lpf >= 98)
data = INV_FILTER_98HZ;
else if (lpf >= 42)
data = INV_FILTER_42HZ;
else if (lpf >= 20)
data = INV_FILTER_20HZ;
else if (lpf >= 10)
data = INV_FILTER_10HZ;
else
data = INV_FILTER_5HZ;
if (st.chip_cfg.lpf == data)
return 0;
if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
return -1;
st.chip_cfg.lpf = data;
return 0;
}
/**
* @brief Get sampling rate.
* @param[out] rate Current sampling rate (Hz).
* @return 0 if successful.
*/
int mpu_get_sample_rate(unsigned short *rate)
{
if (st.chip_cfg.dmp_on)
return -1;
else
rate[0] = st.chip_cfg.sample_rate;
return 0;
}
/**
* @brief Set sampling rate.
* Sampling rate must be between 4Hz and 1kHz.
* @param[in] rate Desired sampling rate (Hz).
* @return 0 if successful.
*/
int mpu_set_sample_rate(unsigned short rate)
{
unsigned char data;
if (!(st.chip_cfg.sensors))
return -1;
if (st.chip_cfg.dmp_on)
return -1;
else {
if (st.chip_cfg.lp_accel_mode) {
if (rate && (rate <= 40)) {
/* Just stay in low-power accel mode. */
mpu_lp_accel_mode(rate);
return 0;
}
/* Requested rate exceeds the allowed frequencies in LP accel mode,
* switch back to full-power mode.
*/
mpu_lp_accel_mode(0);
}
if (rate < 4)
rate = 4;
else if (rate > 1000)
rate = 1000;
data = 1000 / rate - 1;
if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))
return -1;
st.chip_cfg.sample_rate = 1000 / (1 + data);
#ifdef AK89xx_SECONDARY
mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
#endif
/* Automatically set LPF to 1/2 sampling rate. */
mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
return 0;
}
}
/**
* @brief Get compass sampling rate.
* @param[out] rate Current compass sampling rate (Hz).
* @return 0 if successful.
*/
int mpu_get_compass_sample_rate(unsigned short *rate)
{
#ifdef AK89xx_SECONDARY
rate[0] = st.chip_cfg.compass_sample_rate;
return 0;
#else
rate[0] = 0;
return -1;
#endif
}
/**
* @brief Set compass sampling rate.
* The compass on the auxiliary I2C bus is read by the MPU hardware at a
* maximum of 100Hz. The actual rate can be set to a fraction of the gyro
* sampling rate.
*
* \n WARNING: The new rate may be different than what was requested. Call
* mpu_get_compass_sample_rate to check the actual setting.
* @param[in] rate Desired compass sampling rate (Hz).
* @return 0 if successful.
*/
int mpu_set_compass_sample_rate(unsigned short rate)
{
#ifdef AK89xx_SECONDARY
unsigned char div;
if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
return -1;
div = st.chip_cfg.sample_rate / rate - 1;
if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
return -1;
st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
return 0;
#else
return -1;
#endif
}
/**
* @brief Get gyro sensitivity scale factor.
* @param[out] sens Conversion from hardware units to dps.
* @return 0 if successful.
*/
int mpu_get_gyro_sens(float *sens)
{
switch (st.chip_cfg.gyro_fsr) {
case INV_FSR_250DPS:
sens[0] = 131.f;
break;
case INV_FSR_500DPS:
sens[0] = 65.5f;
break;
case INV_FSR_1000DPS:
sens[0] = 32.8f;
break;
case INV_FSR_2000DPS:
sens[0] = 16.4f;
break;
default:
return -1;
}
return 0;
}
/**
* @brief Get accel sensitivity scale factor.
* @param[out] sens Conversion from hardware units to g's.
* @return 0 if successful.
*/
int mpu_get_accel_sens(unsigned short *sens)
{
switch (st.chip_cfg.accel_fsr) {
case INV_FSR_2G:
sens[0] = 16384;
break;
case INV_FSR_4G:
sens[0] = 8092;
break;
case INV_FSR_8G:
sens[0] = 4096;
break;
case INV_FSR_16G:
sens[0] = 2048;
break;
default:
return -1;
}
if (st.chip_cfg.accel_half)
sens[0] >>= 1;
return 0;
}
/**
* @brief Get current FIFO configuration.
* @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* @param[out] sensors Mask of sensors in FIFO.
* @return 0 if successful.
*/
int mpu_get_fifo_config(unsigned char *sensors)
{
sensors[0] = st.chip_cfg.fifo_enable;
return 0;
}
/**
* @brief Select which sensors are pushed to FIFO.
* @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* @param[in] sensors Mask of sensors to push to FIFO.
* @return 0 if successful.
*/
int mpu_configure_fifo(unsigned char sensors)
{
unsigned char prev;
int result = 0;
/* Compass data isn't going into the FIFO. Stop trying. */
sensors &= ~INV_XYZ_COMPASS;
if (st.chip_cfg.dmp_on)
return 0;
else {
if (!(st.chip_cfg.sensors))
return -1;
prev = st.chip_cfg.fifo_enable;
st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
if (st.chip_cfg.fifo_enable != sensors)
/* You're not getting what you asked for. Some sensors are
* asleep.
*/
result = -1;
else
result = 0;
if (sensors || st.chip_cfg.lp_accel_mode)
set_int_enable(1);
else
set_int_enable(0);
if (sensors) {
if (mpu_reset_fifo()) {
st.chip_cfg.fifo_enable = prev;
return -1;
}
}
}
return result;
}
/**
* @brief Get current power state.
* @param[in] power_on 1 if turned on, 0 if suspended.
* @return 0 if successful.
*/
int mpu_get_power_state(unsigned char *power_on)
{
if (st.chip_cfg.sensors)
power_on[0] = 1;
else
power_on[0] = 0;
return 0;
}
/**
* @brief Turn specific sensors on/off.
* @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n INV_XYZ_COMPASS
* @param[in] sensors Mask of sensors to wake.
* @return 0 if successful.
*/
int mpu_set_sensors(unsigned char sensors)
{
unsigned char data;
#ifdef AK89xx_SECONDARY
unsigned char user_ctrl;
#endif
if (sensors & INV_XYZ_GYRO)
data = INV_CLK_PLL;
else if (sensors)
data = 0;
else
data = BIT_SLEEP;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
st.chip_cfg.sensors = 0;
return -1;
}
st.chip_cfg.clk_src = data & ~BIT_SLEEP;
data = 0;
if (!(sensors & INV_X_GYRO))
data |= BIT_STBY_XG;
if (!(sensors & INV_Y_GYRO))
data |= BIT_STBY_YG;
if (!(sensors & INV_Z_GYRO))
data |= BIT_STBY_ZG;
if (!(sensors & INV_XYZ_ACCEL))
data |= BIT_STBY_XYZA;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
st.chip_cfg.sensors = 0;
return -1;
}
if (sensors && (sensors != INV_XYZ_ACCEL))
/* Latched interrupts only used in LP accel mode. */
mpu_set_int_latched(0);
#ifdef AK89xx_SECONDARY
#ifdef AK89xx_BYPASS
if (sensors & INV_XYZ_COMPASS)
mpu_set_bypass(1);
else
mpu_set_bypass(0);
#else
if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
return -1;
/* Handle AKM power management. */
if (sensors & INV_XYZ_COMPASS) {
data = AKM_SINGLE_MEASUREMENT;
user_ctrl |= BIT_AUX_IF_EN;
} else {
data = AKM_POWER_DOWN;
user_ctrl &= ~BIT_AUX_IF_EN;
}
if (st.chip_cfg.dmp_on)
user_ctrl |= BIT_DMP_EN;
else
user_ctrl &= ~BIT_DMP_EN;
if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
return -1;
/* Enable/disable I2C master mode. */
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
return -1;
#endif
#endif
st.chip_cfg.sensors = sensors;
st.chip_cfg.lp_accel_mode = 0;
delay_ms(50);
return 0;
}
/**
* @brief Read the MPU interrupt status registers.
* @param[out] status Mask of interrupt bits.
* @return 0 if successful.
*/
int mpu_get_int_status(short *status)
{
unsigned char tmp[2];
if (!st.chip_cfg.sensors)
return -1;
if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
return -1;
status[0] = (tmp[0] << 8) | tmp[1];
return 0;
}
/**
* @brief Get one packet from the FIFO.
* If @e sensors does not contain a particular sensor, disregard the data
* returned to that pointer.
* \n @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n If the FIFO has no new data, @e sensors will be zero.
* \n If the FIFO is disabled, @e sensors will be zero and this function will
* return a non-zero error code.
* @param[out] gyro Gyro data in hardware units.
* @param[out] accel Accel data in hardware units.
* @param[out] timestamp Timestamp in milliseconds.
* @param[out] sensors Mask of sensors read from FIFO.
* @param[out] more Number of remaining packets.
* @return 0 if successful.
*/
int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
unsigned char *sensors, unsigned char *more)
{
/* Assumes maximum packet size is gyro (6) + accel (6). */
unsigned char data[MAX_PACKET_LENGTH];
unsigned char packet_size = 0;
unsigned short fifo_count, index = 0;
if (st.chip_cfg.dmp_on)
return -1;
sensors[0] = 0;
if (!st.chip_cfg.sensors)
return -1;
if (!st.chip_cfg.fifo_enable)
return -1;
if (st.chip_cfg.fifo_enable & INV_X_GYRO)
packet_size += 2;
if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
packet_size += 2;
if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
packet_size += 2;
if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
packet_size += 6;
if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
return -1;
fifo_count = (data[0] << 8) | data[1];
if (fifo_count < packet_size)
return 0;
//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[index+0] << 8) | data[index+1];
accel[1] = (data[index+2] << 8) | data[index+3];
accel[2] = (data[index+4] << 8) | data[index+5];
sensors[0] |= INV_XYZ_ACCEL;
index += 6;
}
if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
gyro[0] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_X_GYRO;
index += 2;
}
if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
gyro[1] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_Y_GYRO;
index += 2;
}
if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
gyro[2] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_Z_GYRO;
index += 2;
}
return 0;
}
/**
* @brief Get one unparsed packet from the FIFO.
* This function should be used if the packet is to be parsed elsewhere.
* @param[in] length Length of one FIFO packet.
* @param[in] data FIFO packet.
* @param[in] more Number of remaining packets.
*/
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
unsigned char *more)
{
unsigned char tmp[2];
unsigned short fifo_count;
if (!st.chip_cfg.dmp_on)
return -1;
if (!st.chip_cfg.sensors)
return -1;
if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
return -1;
fifo_count = (tmp[0] << 8) | tmp[1];
if (fifo_count < length) {
more[0] = 0;
return -1;
}
if (fifo_count > (st.hw->max_fifo >> 1)) {
/* FIFO is 50% full, better check overflow bit. */
if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
return -1;
if (tmp[0] & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo();
return -2;
}
}
if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
return -1;
more[0] = fifo_count / length - 1;
return 0;
}
/**
* @brief Set device to bypass mode.
* @param[in] bypass_on 1 to enable bypass mode.
* @return 0 if successful.
*/
int mpu_set_bypass(unsigned char bypass_on)
{
unsigned char tmp;
if (st.chip_cfg.bypass_mode == bypass_on)
return 0;
if (bypass_on) {
if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
return -1;
tmp &= ~BIT_AUX_IF_EN;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
return -1;
delay_ms(3);
tmp = BIT_BYPASS_EN;
if (st.chip_cfg.active_low_int)
tmp |= BIT_ACTL;
if (st.chip_cfg.latched_int)
tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
return -1;
} else {
/* Enable I2C master mode if compass is being used. */
if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
return -1;
if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
tmp |= BIT_AUX_IF_EN;
else
tmp &= ~BIT_AUX_IF_EN;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
return -1;
delay_ms(3);
if (st.chip_cfg.active_low_int)
tmp = BIT_ACTL;
else
tmp = 0;
if (st.chip_cfg.latched_int)
tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
return -1;
}
st.chip_cfg.bypass_mode = bypass_on;
return 0;
}
/**
* @brief Set interrupt level.
* @param[in] active_low 1 for active low, 0 for active high.
* @return 0 if successful.
*/
int mpu_set_int_level(unsigned char active_low)
{
st.chip_cfg.active_low_int = active_low;
return 0;
}
/**
* @brief Enable latched interrupts.
* Any MPU register will clear the interrupt.
* @param[in] enable 1 to enable, 0 to disable.
* @return 0 if successful.
*/
int mpu_set_int_latched(unsigned char enable)
{
unsigned char tmp;
if (st.chip_cfg.latched_int == enable)
return 0;
if (enable)
tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
else
tmp = 0;
if (st.chip_cfg.bypass_mode)
tmp |= BIT_BYPASS_EN;
if (st.chip_cfg.active_low_int)
tmp |= BIT_ACTL;
if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
return -1;
st.chip_cfg.latched_int = enable;
return 0;
}
#ifdef MPU6050
static int get_accel_prod_shift(float *st_shift)
{
unsigned char tmp[4], shift_code[3], ii;
if (i2c_read(st.hw->addr, 0x0D, 4, tmp))
return 0x07;
shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
for (ii = 0; ii < 3; ii++) {
if (!shift_code[ii]) {
st_shift[ii] = 0.f;
continue;
}
/* Equivalent to..
* st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
*/
st_shift[ii] = 0.34f;
while (--shift_code[ii])
st_shift[ii] *= 1.034f;
}
return 0;
}
static int accel_self_test(long *bias_regular, long *bias_st)
{
int jj, result = 0;
float st_shift[3], st_shift_cust, st_shift_var;
get_accel_prod_shift(st_shift);
for(jj = 0; jj < 3; jj++) {
st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
if (st_shift[jj]) {
st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
if (fabs(st_shift_var) > test.max_accel_var)
result |= 1 << jj;
} else if ((st_shift_cust < test.min_g) ||
(st_shift_cust > test.max_g))
result |= 1 << jj;
}
return result;
}
static int gyro_self_test(long *bias_regular, long *bias_st)
{
int jj, result = 0;
unsigned char tmp[3];
float st_shift, st_shift_cust, st_shift_var;
if (i2c_read(st.hw->addr, 0x0D, 3, tmp))
return 0x07;
tmp[0] &= 0x1F;
tmp[1] &= 0x1F;
tmp[2] &= 0x1F;
for (jj = 0; jj < 3; jj++) {
st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
if (tmp[jj]) {
st_shift = 3275.f / test.gyro_sens;
while (--tmp[jj])
st_shift *= 1.046f;
st_shift_var = st_shift_cust / st_shift - 1.f;
if (fabs(st_shift_var) > test.max_gyro_var)
result |= 1 << jj;
} else if ((st_shift_cust < test.min_dps) ||
(st_shift_cust > test.max_dps))
result |= 1 << jj;
}
return result;
}
#ifdef AK89xx_SECONDARY
static int compass_self_test(void)
{
unsigned char tmp[6];
unsigned char tries = 10;
int result = 0x07;
short data;
mpu_set_bypass(1);
tmp[0] = AKM_POWER_DOWN;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
return 0x07;
tmp[0] = AKM_BIT_SELF_TEST;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
goto AKM_restore;
tmp[0] = AKM_MODE_SELF_TEST;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
goto AKM_restore;
do {
delay_ms(10);
if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
goto AKM_restore;
if (tmp[0] & AKM_DATA_READY)
break;
} while (tries--);
if (!(tmp[0] & AKM_DATA_READY))
goto AKM_restore;
if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
goto AKM_restore;
result = 0;
data = (short)(tmp[1] << 8) | tmp[0];
if ((data > 100) || (data < -100))
result |= 0x01;
data = (short)(tmp[3] << 8) | tmp[2];
if ((data > 100) || (data < -100))
result |= 0x02;
data = (short)(tmp[5] << 8) | tmp[4];
if ((data > -300) || (data < -1000))
result |= 0x04;
AKM_restore:
tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
mpu_set_bypass(0);
return result;
}
#endif
#endif
static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
{
unsigned char data[MAX_PACKET_LENGTH];
unsigned char packet_count, ii;
unsigned short fifo_count;
data[0] = 0x01;
data[1] = 0;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
return -1;
delay_ms(200);
data[0] = 0;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
return -1;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
return -1;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
return -1;
if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
return -1;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
return -1;
data[0] = BIT_FIFO_RST | BIT_DMP_RST;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
return -1;
delay_ms(15);
data[0] = st.test->reg_lpf;
if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
return -1;
data[0] = st.test->reg_rate_div;
if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
return -1;
if (hw_test)
data[0] = st.test->reg_gyro_fsr | 0xE0;
else
data[0] = st.test->reg_gyro_fsr;
if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
return -1;
if (hw_test)
data[0] = st.test->reg_accel_fsr | 0xE0;
else
data[0] = test.reg_accel_fsr;
if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
return -1;
if (hw_test)
delay_ms(200);
/* Fill FIFO for test.wait_ms milliseconds. */
data[0] = BIT_FIFO_EN;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
return -1;
data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
return -1;
delay_ms(test.wait_ms);
data[0] = 0;
if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
return -1;
if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
return -1;
fifo_count = (data[0] << 8) | data[1];
packet_count = fifo_count / MAX_PACKET_LENGTH;
gyro[0] = gyro[1] = gyro[2] = 0;
accel[0] = accel[1] = accel[2] = 0;
for (ii = 0; ii < packet_count; ii++) {
short accel_cur[3], gyro_cur[3];
if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
return -1;
accel_cur[0] = ((short)data[0] << 8) | data[1];
accel_cur[1] = ((short)data[2] << 8) | data[3];
accel_cur[2] = ((short)data[4] << 8) | data[5];
accel[0] += (long)accel_cur[0];
accel[1] += (long)accel_cur[1];
accel[2] += (long)accel_cur[2];
gyro_cur[0] = (((short)data[6] << 8) | data[7]);
gyro_cur[1] = (((short)data[8] << 8) | data[9]);
gyro_cur[2] = (((short)data[10] << 8) | data[11]);
gyro[0] += (long)gyro_cur[0];
gyro[1] += (long)gyro_cur[1];
gyro[2] += (long)gyro_cur[2];
}
#ifdef EMPL_NO_64BIT
gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
if (has_accel) {
accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
packet_count);
accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
packet_count);
accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
packet_count);
/* Don't remove gravity! */
accel[2] -= 65536L;
}
#else
gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
packet_count);
accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
packet_count);
accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
packet_count);
/* Don't remove gravity! */
if (accel[2] > 0L)
accel[2] -= 65536L;
else
accel[2] += 65536L;
#endif
return 0;
}
/**
* @brief Trigger gyro/accel/compass self-test.
* On success/error, the self-test returns a mask representing the sensor(s)
* that failed. For each bit, a one (1) represents a "pass" case; conversely,
* a zero (0) indicates a failure.
*
* \n The mask is defined as follows:
* \n Bit 0: Gyro.
* \n Bit 1: Accel.
* \n Bit 2: Compass.
*
* \n Currently, the hardware self-test is unsupported for MPU6500. However,
* this function can still be used to obtain the accel and gyro biases.
*
* \n This function must be called with the device either face-up or face-down
* (z-axis is parallel to gravity).
* @param[out] gyro Gyro biases in q16 format.
* @param[out] accel Accel biases (if applicable) in q16 format.
* @return Result mask (see above).
*/
int mpu_run_self_test(long *gyro, long *accel)
{
#ifdef MPU6050
const unsigned char tries = 2;
long gyro_st[3], accel_st[3];
unsigned char accel_result, gyro_result;
#ifdef AK89xx_SECONDARY
unsigned char compass_result;
#endif
int ii;
#endif
int result;
unsigned char accel_fsr, fifo_sensors, sensors_on;
unsigned short gyro_fsr, sample_rate, lpf;
unsigned char dmp_was_on;
if (st.chip_cfg.dmp_on) {
mpu_set_dmp_state(0);
dmp_was_on = 1;
} else
dmp_was_on = 0;
/* Get initial settings. */
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_lpf(&lpf);
mpu_get_sample_rate(&sample_rate);
sensors_on = st.chip_cfg.sensors;
mpu_get_fifo_config(&fifo_sensors);
/* For older chips, the self-test will be different. */
#if defined MPU6050
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro, accel, 0))
break;
if (ii == tries) {
/* If we reach this point, we most likely encountered an I2C error.
* We'll just report an error for all three sensors.
*/
result = 0;
goto restore;
}
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro_st, accel_st, 1))
break;
if (ii == tries) {
/* Again, probably an I2C error. */
result = 0;
goto restore;
}
accel_result = accel_self_test(accel, accel_st);
gyro_result = gyro_self_test(gyro, gyro_st);
result = 0;
if (!gyro_result)
result |= 0x01;
if (!accel_result)
result |= 0x02;
#ifdef AK89xx_SECONDARY
compass_result = compass_self_test();
if (!compass_result)
result |= 0x04;
#endif
restore:
#elif defined MPU6500
/* For now, this function will return a "pass" result for all three sensors
* for compatibility with current test applications.
*/
get_st_biases(gyro, accel, 0);
result = 0x7;
#endif
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.clk_src = INV_CLK_PLL;
mpu_set_gyro_fsr(gyro_fsr);
mpu_set_accel_fsr(accel_fsr);
mpu_set_lpf(lpf);
mpu_set_sample_rate(sample_rate);
mpu_set_sensors(sensors_on);
mpu_configure_fifo(fifo_sensors);
if (dmp_was_on)
mpu_set_dmp_state(1);
return result;
}
/**
* @brief Write to the DMP memory.
* This function prevents I2C writes past the bank boundaries. The DMP memory
* is only accessible when the chip is awake.
* @param[in] mem_addr Memory location (bank << 8 | start address)
* @param[in] length Number of bytes to write.
* @param[in] data Bytes to write to memory.
* @return 0 if successful.
*/
int mpu_write_mem(unsigned short mem_addr, unsigned short length,
unsigned char *data)
{
unsigned char tmp[2];
if (!data)
return -1;
if (!st.chip_cfg.sensors)
return -1;
tmp[0] = (unsigned char)(mem_addr >> 8);
tmp[1] = (unsigned char)(mem_addr & 0xFF);
/* Check bank boundaries. */
if (tmp[1] + length > st.hw->bank_size)
return -1;
if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
return -1;
if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
return -1;
return 0;
}
/**
* @brief Read from the DMP memory.
* This function prevents I2C reads past the bank boundaries. The DMP memory
* is only accessible when the chip is awake.
* @param[in] mem_addr Memory location (bank << 8 | start address)
* @param[in] length Number of bytes to read.
* @param[out] data Bytes read from memory.
* @return 0 if successful.
*/
int mpu_read_mem(unsigned short mem_addr, unsigned short length,
unsigned char *data)
{
unsigned char tmp[2];
if (!data)
return -1;
if (!st.chip_cfg.sensors)
return -1;
tmp[0] = (unsigned char)(mem_addr >> 8);
tmp[1] = (unsigned char)(mem_addr & 0xFF);
/* Check bank boundaries. */
if (tmp[1] + length > st.hw->bank_size)
return -1;
if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
return -1;
if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
return -1;
return 0;
}
/**
* @brief Load and verify DMP image.
* @param[in] length Length of DMP image.
* @param[in] firmware DMP code.
* @param[in] start_addr Starting address of DMP code memory.
* @param[in] sample_rate Fixed sampling rate used when DMP is enabled.
* @return 0 if successful.
*/
int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
unsigned short start_addr, unsigned short sample_rate)
{
unsigned short ii;
unsigned short this_write;
/* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
#define LOAD_CHUNK (16)
unsigned char cur[LOAD_CHUNK], tmp[2];
if (st.chip_cfg.dmp_loaded)
/* DMP should only be loaded once. */
return -1;
if (!firmware)
return -1;
for (ii = 0; ii < length; ii += this_write) {
this_write = min(LOAD_CHUNK, length - ii);
if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
return -1;
if (mpu_read_mem(ii, this_write, cur))
return -1;
if (memcmp(firmware+ii, cur, this_write))
return -2;
}
/* Set program start address. */
tmp[0] = start_addr >> 8;
tmp[1] = start_addr & 0xFF;
if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
return -1;
st.chip_cfg.dmp_loaded = 1;
st.chip_cfg.dmp_sample_rate = sample_rate;
return 0;
}
/**
* @brief Enable/disable DMP support.
* @param[in] enable 1 to turn on the DMP.
* @return 0 if successful.
*/
int mpu_set_dmp_state(unsigned char enable)
{
unsigned char tmp;
if (st.chip_cfg.dmp_on == enable)
return 0;
if (enable) {
if (!st.chip_cfg.dmp_loaded)
return -1;
/* Disable data ready interrupt. */
set_int_enable(0);
/* Disable bypass mode. */
mpu_set_bypass(0);
/* Keep constant sample rate, FIFO rate controlled by DMP. */
mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
/* Remove FIFO elements. */
tmp = 0;
i2c_write(st.hw->addr, 0x23, 1, &tmp);
st.chip_cfg.dmp_on = 1;
/* Enable DMP interrupt. */
set_int_enable(1);
mpu_reset_fifo();
} else {
/* Disable DMP interrupt. */
set_int_enable(0);
/* Restore FIFO settings. */
tmp = st.chip_cfg.fifo_enable;
i2c_write(st.hw->addr, 0x23, 1, &tmp);
st.chip_cfg.dmp_on = 0;
mpu_reset_fifo();
}
return 0;
}
/**
* @brief Get DMP state.
* @param[out] enabled 1 if enabled.
* @return 0 if successful.
*/
int mpu_get_dmp_state(unsigned char *enabled)
{
enabled[0] = st.chip_cfg.dmp_on;
return 0;
}
/* This initialization is similar to the one in ak8975.c. */
int setup_compass(void)
{
#ifdef AK89xx_SECONDARY
unsigned char data[4], akm_addr;
mpu_set_bypass(1);
/* Find compass. Possible addresses range from 0x0C to 0x0F. */
for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
int result;
result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
if (!result && (data[0] == AKM_WHOAMI))
break;
}
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
//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;
#endif
return 0;
#else
return -1;
#endif
}
/**
* @brief Read raw compass data.
* @param[out] data Raw data in hardware units.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_compass_reg(short *data, unsigned long *timestamp)
{
#ifdef AK89xx_SECONDARY
unsigned char tmp[9];
if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
return -1;
#ifdef AK89xx_BYPASS
if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
return -1;
tmp[8] = AKM_SINGLE_MEASUREMENT;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
return -1;
#else
if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
return -1;
#endif
#if defined AK8975_SECONDARY
/* AK8975 doesn't have the overrun error bit. */
if (!(tmp[0] & AKM_DATA_READY))
return -2;
if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
return -3;
#elif defined AK8963_SECONDARY
/* AK8963 doesn't have the data read error bit. */
if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
return -2;
if (tmp[7] & AKM_OVERFLOW)
return -3;
#endif
data[0] = (tmp[2] << 8) | tmp[1];
data[1] = (tmp[4] << 8) | tmp[3];
data[2] = (tmp[6] << 8) | tmp[5];
data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
if (timestamp)
get_ms(timestamp);
return 0;
#else
return -1;
#endif
}
/**
* @brief Get the compass full-scale range.
* @param[out] fsr Current full-scale range.
* @return 0 if successful.
*/
int mpu_get_compass_fsr(unsigned short *fsr)
{
#ifdef AK89xx_SECONDARY
fsr[0] = st.hw->compass_fsr;
return 0;
#else
return -1;
#endif
}
/**
* @brief Enters LP accel motion interrupt mode.
* The behavior of this feature is very different between the MPU6050 and the
* MPU6500. Each chip's version of this feature is explained below.
*
* \n MPU6050:
* \n When this mode is first enabled, the hardware captures a single accel
* sample, and subsequent samples are compared with this one to determine if
* the device is in motion. Therefore, whenever this "locked" sample needs to
* be changed, this function must be called again.
*
* \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
* increments.
*
* \n Low-power accel mode supports the following frequencies:
* \n 1.25Hz, 5Hz, 20Hz, 40Hz
*
* \n MPU6500:
* \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
* sample. The hardware monitors the accel data and detects any large change
* over a short period of time.
*
* \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
* increments.
*
* \n MPU6500 Low-power accel mode supports the following frequencies:
* \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
*
* \n\n NOTES:
* \n The driver will round down @e thresh to the nearest supported value if
* an unsupported threshold is selected.
* \n To select a fractional wake-up frequency, round down the value passed to
* @e lpa_freq.
* \n The MPU6500 does not support a delay parameter. If this function is used
* for the MPU6500, the value passed to @e time will be ignored.
* \n To disable this mode, set @e lpa_freq to zero. The driver will restore
* the previous configuration.
*
* @param[in] thresh Motion threshold in mg.
* @param[in] time Duration in milliseconds that the accel data must
* exceed @e thresh before motion is reported.
* @param[in] lpa_freq Minimum sampling rate, or zero to disable.
* @return 0 if successful.
*/
int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
unsigned char lpa_freq)
{
unsigned char data[3];
if (lpa_freq) {
unsigned char thresh_hw;
#if defined MPU6050
/* TODO: Make these const/#defines. */
/* 1LSb = 32mg. */
if (thresh > 8160)
thresh_hw = 255;
else if (thresh < 32)
thresh_hw = 1;
else
thresh_hw = thresh >> 5;
#elif defined MPU6500
/* 1LSb = 4mg. */
if (thresh > 1020)
thresh_hw = 255;
else if (thresh < 4)
thresh_hw = 1;
else
thresh_hw = thresh >> 2;
#endif
if (!time)
/* Minimum duration must be 1ms. */
time = 1;
#if defined MPU6050
if (lpa_freq > 40)
#elif defined MPU6500
if (lpa_freq > 640)
#endif
/* At this point, the chip has not been re-configured, so the
* function can safely exit.
*/
return -1;
if (!st.chip_cfg.int_motion_only) {
/* Store current settings for later. */
if (st.chip_cfg.dmp_on) {
mpu_set_dmp_state(0);
st.chip_cfg.cache.dmp_on = 1;
} else
st.chip_cfg.cache.dmp_on = 0;
mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
mpu_get_lpf(&st.chip_cfg.cache.lpf);
mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
}
#ifdef MPU6050
/* Disable hardware interrupts for now. */
set_int_enable(0);
/* Enter full-power accel-only mode. */
mpu_lp_accel_mode(0);
/* Override current LPF (and HPF) settings to obtain a valid accel
* reading.
*/
data[0] = INV_FILTER_256HZ_NOLPF2;
if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
return -1;
/* NOTE: Digital high pass filter should be configured here. Since this
* driver doesn't modify those bits anywhere, they should already be
* cleared by default.
*/
/* Configure the device to send motion interrupts. */
/* Enable motion interrupt. */
data[0] = BIT_MOT_INT_EN;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
goto lp_int_restore;
/* Set motion interrupt parameters. */
data[0] = thresh_hw;
data[1] = time;
if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data))
goto lp_int_restore;
/* Force hardware to "lock" current accel sample. */
delay_ms(5);
data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF;
if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
goto lp_int_restore;
/* Set up LP accel mode. */
data[0] = BIT_LPA_CYCLE;
if (lpa_freq == 1)
data[1] = INV_LPA_1_25HZ;
else if (lpa_freq <= 5)
data[1] = INV_LPA_5HZ;
else if (lpa_freq <= 20)
data[1] = INV_LPA_20HZ;
else
data[1] = INV_LPA_40HZ;
data[1] = (data[1] << 6) | BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
goto lp_int_restore;
st.chip_cfg.int_motion_only = 1;
return 0;
#elif defined MPU6500
/* Disable hardware interrupts. */
set_int_enable(0);
/* Enter full-power accel-only mode, no FIFO/DMP. */
data[0] = 0;
data[1] = 0;
data[2] = BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
goto lp_int_restore;
/* Set motion threshold. */
data[0] = thresh_hw;
if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
goto lp_int_restore;
/* Set wake frequency. */
if (lpa_freq == 1)
data[0] = INV_LPA_1_25HZ;
else if (lpa_freq == 2)
data[0] = INV_LPA_2_5HZ;
else if (lpa_freq <= 5)
data[0] = INV_LPA_5HZ;
else if (lpa_freq <= 10)
data[0] = INV_LPA_10HZ;
else if (lpa_freq <= 20)
data[0] = INV_LPA_20HZ;
else if (lpa_freq <= 40)
data[0] = INV_LPA_40HZ;
else if (lpa_freq <= 80)
data[0] = INV_LPA_80HZ;
else if (lpa_freq <= 160)
data[0] = INV_LPA_160HZ;
else if (lpa_freq <= 320)
data[0] = INV_LPA_320HZ;
else
data[0] = INV_LPA_640HZ;
if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
goto lp_int_restore;
/* Enable motion interrupt (MPU6500 version). */
data[0] = BITS_WOM_EN;
if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
goto lp_int_restore;
/* Enable cycle mode. */
data[0] = BIT_LPA_CYCLE;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
goto lp_int_restore;
/* Enable interrupt. */
data[0] = BIT_MOT_INT_EN;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
goto lp_int_restore;
st.chip_cfg.int_motion_only = 1;
return 0;
#endif
} else {
/* Don't "restore" the previous state if no state has been saved. */
int ii;
char *cache_ptr = (char*)&st.chip_cfg.cache;
for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
if (cache_ptr[ii] != 0)
goto lp_int_restore;
}
/* If we reach this point, motion interrupt mode hasn't been used yet. */
return -1;
}
lp_int_restore:
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.clk_src = INV_CLK_PLL;
mpu_set_sensors(st.chip_cfg.cache.sensors_on);
mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
mpu_set_lpf(st.chip_cfg.cache.lpf);
mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
if (st.chip_cfg.cache.dmp_on)
mpu_set_dmp_state(1);
#ifdef MPU6500
/* Disable motion interrupt (MPU6500 version). */
data[0] = 0;
if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
goto lp_int_restore;
#endif
st.chip_cfg.int_motion_only = 0;
return 0;
}
//q30格式,long转float时的除数.
#define q30 1073741824.0f
//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1
};
//MPU6050自测试
//返回值:0,正常
// 其他,失败
uint8_t run_self_test(void)
{
int result;
//char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3)
{
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
return 0;
} else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
/*
XYZ 010_001_000 Identity Matrix
XZY 001_010_000
YXZ 010_000_001
YZX 000_010_001
ZXY 001_000_010
ZYX 000_001_010
*/
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{
}
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_init(void)
{
uint8_t res=0;
MPU_IIC_Init(); //初始化IIC总线
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
// if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
} else return 10;
return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
} else return 2;
return 0;
}
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 "stm32f1xx_hal.h"
//定义输出速度
#define DEFAULT_MPU_HZ (100) //100Hz
#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_MSP430
void (*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);
uint8_t run_self_test(void);
uint8_t mpu_dmp_init(void);
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw);
#endif /* #ifndef _INV_MPU_H_ */
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"
//定义目标板采用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, we're using
* a TWI driver that follows the slave address + register address convention.
*/
#include "delay.h"
#include "sysclk.h"
#include "log.h"
#include "uc3l0_clock.h"
/* delay_ms is a function already defined in ASF. */
#define get_ms uc3l0_get_clock_ms
#define 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 (22+512)
#define D_0_24 (24+512)
#define D_0_36 (36)
#define D_0_52 (52)
#define D_0_96 (96)
#define D_0_104 (104)
#define D_0_108 (108)
#define D_0_163 (163)
#define D_0_188 (188)
#define D_0_192 (192)
#define D_0_224 (224)
#define D_0_228 (228)
#define D_0_232 (232)
#define D_0_236 (236)
#define D_1_2 (256 + 2)
#define D_1_4 (256 + 4)
#define D_1_8 (256 + 8)
#define D_1_10 (256 + 10)
#define D_1_24 (256 + 24)
#define D_1_28 (256 + 28)
#define D_1_36 (256 + 36)
#define D_1_40 (256 + 40)
#define D_1_44 (256 + 44)
#define D_1_72 (256 + 72)
#define D_1_74 (256 + 74)
#define D_1_79 (256 + 79)
#define D_1_88 (256 + 88)
#define D_1_90 (256 + 90)
#define D_1_92 (256 + 92)
#define D_1_96 (256 + 96)
#define D_1_98 (256 + 98)
#define D_1_106 (256 + 106)
#define D_1_108 (256 + 108)
#define D_1_112 (256 + 112)
#define D_1_128 (256 + 144)
#define D_1_152 (256 + 12)
#define D_1_160 (256 + 160)
#define D_1_176 (256 + 176)
#define D_1_178 (256 + 178)
#define D_1_218 (256 + 218)
#define D_1_232 (256 + 232)
#define D_1_236 (256 + 236)
#define D_1_240 (256 + 240)
#define D_1_244 (256 + 244)
#define D_1_250 (256 + 250)
#define D_1_252 (256 + 252)
#define D_2_12 (512 + 12)
#define D_2_96 (512 + 96)
#define D_2_108 (512 + 108)
#define D_2_208 (512 + 208)
#define D_2_224 (512 + 224)
#define D_2_236 (512 + 236)
#define D_2_244 (512 + 244)
#define D_2_248 (512 + 248)
#define D_2_252 (512 + 252)
#define CPASS_BIAS_X (35 * 16 + 4)
#define CPASS_BIAS_Y (35 * 16 + 8)
#define CPASS_BIAS_Z (35 * 16 + 12)
#define CPASS_MTX_00 (36 * 16)
#define CPASS_MTX_01 (36 * 16 + 4)
#define CPASS_MTX_02 (36 * 16 + 8)
#define CPASS_MTX_10 (36 * 16 + 12)
#define CPASS_MTX_11 (37 * 16)
#define CPASS_MTX_12 (37 * 16 + 4)
#define CPASS_MTX_20 (37 * 16 + 8)
#define CPASS_MTX_21 (37 * 16 + 12)
#define CPASS_MTX_22 (43 * 16 + 12)
#define D_EXT_GYRO_BIAS_X (61 * 16)
#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4
#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8
#define D_ACT0 (40 * 16)
#define D_ACSX (40 * 16 + 4)
#define D_ACSY (40 * 16 + 8)
#define D_ACSZ (40 * 16 + 12)
#define FLICK_MSG (45 * 16 + 4)
#define FLICK_COUNTER (45 * 16 + 8)
#define FLICK_LOWER (45 * 16 + 12)
#define FLICK_UPPER (46 * 16 + 12)
#define D_AUTH_OUT (992)
#define D_AUTH_IN (996)
#define D_AUTH_A (1000)
#define D_AUTH_B (1004)
#define D_PEDSTD_BP_B (768 + 0x1C)
#define D_PEDSTD_HP_A (768 + 0x78)
#define D_PEDSTD_HP_B (768 + 0x7C)
#define D_PEDSTD_BP_A4 (768 + 0x40)
#define D_PEDSTD_BP_A3 (768 + 0x44)
#define D_PEDSTD_BP_A2 (768 + 0x48)
#define D_PEDSTD_BP_A1 (768 + 0x4C)
#define D_PEDSTD_INT_THRSH (768 + 0x68)
#define D_PEDSTD_CLIP (768 + 0x6C)
#define D_PEDSTD_SB (768 + 0x28)
#define D_PEDSTD_SB_TIME (768 + 0x2C)
#define D_PEDSTD_PEAKTHRSH (768 + 0x98)
#define D_PEDSTD_TIML (768 + 0x2A)
#define D_PEDSTD_TIMH (768 + 0x2E)
#define D_PEDSTD_PEAK (768 + 0X94)
#define D_PEDSTD_STEPCTR (768 + 0x60)
#define D_PEDSTD_TIMECTR (964)
#define D_PEDSTD_DECI (768 + 0xA0)
#define D_HOST_NO_MOT (976)
#define D_ACCEL_BIAS (660)
#define D_ORIENT_GAP (76)
#define D_TILT0_H (48)
#define D_TILT0_L (50)
#define D_TILT1_H (52)
#define D_TILT1_L (54)
#define D_TILT2_H (56)
#define D_TILT2_L (58)
#define D_TILT3_H (60)
#define D_TILT3_L (62)
#define DMP_CODE_SIZE (3062)
static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
/* bank # 0 */
0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* bank # 1 */
0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
/* bank # 2 */
0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* bank # 3 */
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* bank # 4 */
0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
/* bank # 5 */
0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
/* bank # 6 */
0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
/* bank # 7 */
0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
/* bank # 8 */
0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
/* bank # 9 */
0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
/* bank # 10 */
0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
/* bank # 11 */
0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
};
static const unsigned short sStartAddress = 0x0400;
/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
#define INT_SRC_TAP (0x01)
#define INT_SRC_ANDROID_ORIENT (0x08)
#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \
DMP_FEATURE_SEND_CAL_GYRO)
#define MAX_PACKET_LENGTH (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 (1L<<24)
#define QUAT_MAG_SQ_NORMALIZED (1L<<28)
#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
#endif
struct dmp_s {
void (*tap_cb)(unsigned char count, unsigned char direction);
void (*android_orient_cb)(unsigned char orientation);
unsigned short orient;
unsigned short feature_mask;
unsigned short fifo_rate;
unsigned char packet_length;
};
//static struct dmp_s dmp = {
// .tap_cb = NULL,
// .android_orient_cb = NULL,
// .orient = 0,
// .feature_mask = 0,
// .fifo_rate = 0,
// .packet_length = 0
//};
static struct dmp_s dmp={
NULL,
NULL,
0,
0,
0,
0
};
/**
* @brief Load the DMP with this image.
* @return 0 if successful.
*/
int dmp_load_motion_driver_firmware(void)
{
return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
DMP_SAMPLE_RATE);
}
/**
* @brief Push gyro and accel orientation to the DMP.
* The orientation is represented here as the output of
* @e inv_orientation_matrix_to_scalar.
* @param[in] orient Gyro and accel orientation in body frame.
* @return 0 if successful.
*/
int dmp_set_orientation(unsigned short orient)
{
unsigned char gyro_regs[3], accel_regs[3];
const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
gyro_regs[0] = gyro_axes[orient & 3];
gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
accel_regs[0] = accel_axes[orient & 3];
accel_regs[1] = accel_axes[(orient >> 3) & 3];
accel_regs[2] = accel_axes[(orient >> 6) & 3];
/* Chip-to-body, axes only. */
if (mpu_write_mem(FCFG_1, 3, gyro_regs))
return -1;
if (mpu_write_mem(FCFG_2, 3, accel_regs))
return -1;
memcpy(gyro_regs, gyro_sign, 3);
memcpy(accel_regs, accel_sign, 3);
if (orient & 4) {
gyro_regs[0] |= 1;
accel_regs[0] |= 1;
}
if (orient & 0x20) {
gyro_regs[1] |= 1;
accel_regs[1] |= 1;
}
if (orient & 0x100) {
gyro_regs[2] |= 1;
accel_regs[2] |= 1;
}
/* Chip-to-body, sign only. */
if (mpu_write_mem(FCFG_3, 3, gyro_regs))
return -1;
if (mpu_write_mem(FCFG_7, 3, accel_regs))
return -1;
dmp.orient = orient;
return 0;
}
/**
* @brief Push gyro biases to the DMP.
* Because the gyro integration is handled in the DMP, any gyro biases
* calculated by the MPL should be pushed down to DMP memory to remove
* 3-axis quaternion drift.
* \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
* overwrite the biases written to this location once a new one is computed.
* @param[in] bias Gyro biases in q16.
* @return 0 if successful.
*/
int dmp_set_gyro_bias(long *bias)
{
long gyro_bias_body[3];
unsigned char regs[4];
gyro_bias_body[0] = bias[dmp.orient & 3];
if (dmp.orient & 4)
gyro_bias_body[0] *= -1;
gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
if (dmp.orient & 0x20)
gyro_bias_body[1] *= -1;
gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
if (dmp.orient & 0x100)
gyro_bias_body[2] *= -1;
#ifdef EMPL_NO_64BIT
gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
#else
gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
#endif
regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
return -1;
regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
return -1;
regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
}
/**
* @brief Push accel biases to the DMP.
* These biases will be removed from the DMP 6-axis quaternion.
* @param[in] bias Accel biases in q16.
* @return 0 if successful.
*/
int dmp_set_accel_bias(long *bias)
{
long accel_bias_body[3];
unsigned char regs[12];
long long accel_sf;
unsigned short accel_sens;
mpu_get_accel_sens(&accel_sens);
accel_sf = (long long)accel_sens << 15;
//__no_operation();
accel_bias_body[0] = bias[dmp.orient & 3];
if (dmp.orient & 4)
accel_bias_body[0] *= -1;
accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
if (dmp.orient & 0x20)
accel_bias_body[1] *= -1;
accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
if (dmp.orient & 0x100)
accel_bias_body[2] *= -1;
#ifdef EMPL_NO_64BIT
accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
#else
accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
#endif
regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
}
/**
* @brief Set DMP output rate.
* Only used when DMP is on.
* @param[in] rate Desired fifo rate (Hz).
* @return 0 if successful.
*/
int dmp_set_fifo_rate(unsigned short rate)
{
const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
unsigned short div;
unsigned char tmp[8];
if (rate > DMP_SAMPLE_RATE)
return -1;
div = DMP_SAMPLE_RATE / rate - 1;
tmp[0] = (unsigned char)((div >> 8) & 0xFF);
tmp[1] = (unsigned char)(div & 0xFF);
if (mpu_write_mem(D_0_22, 2, tmp))
return -1;
if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
return -1;
dmp.fifo_rate = rate;
return 0;
}
/**
* @brief Get DMP output rate.
* @param[out] rate Current fifo rate (Hz).
* @return 0 if successful.
*/
int dmp_get_fifo_rate(unsigned short *rate)
{
rate[0] = dmp.fifo_rate;
return 0;
}
/**
* @brief Set tap threshold for a specific axis.
* @param[in] axis 1, 2, and 4 for XYZ accel, respectively.
* @param[in] thresh Tap threshold, in mg/ms.
* @return 0 if successful.
*/
int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
{
unsigned char tmp[4], accel_fsr;
float scaled_thresh;
unsigned short dmp_thresh, dmp_thresh_2;
if (!(axis & TAP_XYZ) || thresh > 1600)
return -1;
scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
mpu_get_accel_fsr(&accel_fsr);
switch (accel_fsr) {
case 2:
dmp_thresh = (unsigned short)(scaled_thresh * 16384);
/* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
break;
case 4:
dmp_thresh = (unsigned short)(scaled_thresh * 8192);
/* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
break;
case 8:
dmp_thresh = (unsigned short)(scaled_thresh * 4096);
/* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
break;
case 16:
dmp_thresh = (unsigned short)(scaled_thresh * 2048);
/* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
break;
default:
return -1;
}
tmp[0] = (unsigned char)(dmp_thresh >> 8);
tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
if (axis & TAP_X) {
if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
return -1;
if (mpu_write_mem(D_1_36, 2, tmp+2))
return -1;
}
if (axis & TAP_Y) {
if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
return -1;
if (mpu_write_mem(D_1_40, 2, tmp+2))
return -1;
}
if (axis & TAP_Z) {
if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
return -1;
if (mpu_write_mem(D_1_44, 2, tmp+2))
return -1;
}
return 0;
}
/**
* @brief Set which axes will register a tap.
* @param[in] axis 1, 2, and 4 for XYZ, respectively.
* @return 0 if successful.
*/
int dmp_set_tap_axes(unsigned char axis)
{
unsigned char tmp = 0;
if (axis & TAP_X)
tmp |= 0x30;
if (axis & TAP_Y)
tmp |= 0x0C;
if (axis & TAP_Z)
tmp |= 0x03;
return mpu_write_mem(D_1_72, 1, &tmp);
}
/**
* @brief Set minimum number of taps needed for an interrupt.
* @param[in] min_taps Minimum consecutive taps (1-4).
* @return 0 if successful.
*/
int dmp_set_tap_count(unsigned char min_taps)
{
unsigned char tmp;
if (min_taps < 1)
min_taps = 1;
else if (min_taps > 4)
min_taps = 4;
tmp = min_taps - 1;
return mpu_write_mem(D_1_79, 1, &tmp);
}
/**
* @brief Set length between valid taps.
* @param[in] time Milliseconds between taps.
* @return 0 if successful.
*/
int dmp_set_tap_time(unsigned short time)
{
unsigned short dmp_time;
unsigned char tmp[2];
dmp_time = time / (1000 / DMP_SAMPLE_RATE);
tmp[0] = (unsigned char)(dmp_time >> 8);
tmp[1] = (unsigned char)(dmp_time & 0xFF);
return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
}
/**
* @brief Set max time between taps to register as a multi-tap.
* @param[in] time Max milliseconds between taps.
* @return 0 if successful.
*/
int dmp_set_tap_time_multi(unsigned short time)
{
unsigned short dmp_time;
unsigned char tmp[2];
dmp_time = time / (1000 / DMP_SAMPLE_RATE);
tmp[0] = (unsigned char)(dmp_time >> 8);
tmp[1] = (unsigned char)(dmp_time & 0xFF);
return mpu_write_mem(D_1_218, 2, tmp);
}
/**
* @brief Set shake rejection threshold.
* If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
* @param[in] sf Gyro scale factor.
* @param[in] thresh Gyro threshold in dps.
* @return 0 if successful.
*/
int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
{
unsigned char tmp[4];
long thresh_scaled = sf / 1000 * thresh;
tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
return mpu_write_mem(D_1_92, 4, tmp);
}
/**
* @brief Set shake rejection time.
* Sets the length of time that the gyro must be outside of the threshold set
* by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
* 60 ms is added to this parameter.
* @param[in] time Time in milliseconds.
* @return 0 if successful.
*/
int dmp_set_shake_reject_time(unsigned short time)
{
unsigned char tmp[2];
time /= (1000 / DMP_SAMPLE_RATE);
tmp[0] = time >> 8;
tmp[1] = time & 0xFF;
return mpu_write_mem(D_1_90,2,tmp);
}
/**
* @brief Set shake rejection timeout.
* Sets the length of time after a shake rejection that the gyro must stay
* inside of the threshold before taps can be detected again. A mandatory
* 60 ms is added to this parameter.
* @param[in] time Time in milliseconds.
* @return 0 if successful.
*/
int dmp_set_shake_reject_timeout(unsigned short time)
{
unsigned char tmp[2];
time /= (1000 / DMP_SAMPLE_RATE);
tmp[0] = time >> 8;
tmp[1] = time & 0xFF;
return mpu_write_mem(D_1_88,2,tmp);
}
/**
* @brief Get current step count.
* @param[out] count Number of steps detected.
* @return 0 if successful.
*/
int dmp_get_pedometer_step_count(unsigned long *count)
{
unsigned char tmp[4];
if (!count)
return -1;
if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
return -1;
count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
((unsigned long)tmp[2] << 8) | tmp[3];
return 0;
}
/**
* @brief Overwrite current step count.
* WARNING: This function writes to DMP memory and could potentially encounter
* a race condition if called while the pedometer is enabled.
* @param[in] count New step count.
* @return 0 if successful.
*/
int dmp_set_pedometer_step_count(unsigned long count)
{
unsigned char tmp[4];
tmp[0] = (unsigned char)((count >> 24) & 0xFF);
tmp[1] = (unsigned char)((count >> 16) & 0xFF);
tmp[2] = (unsigned char)((count >> 8) & 0xFF);
tmp[3] = (unsigned char)(count & 0xFF);
return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
}
/**
* @brief Get duration of walking time.
* @param[in] time Walk time in milliseconds.
* @return 0 if successful.
*/
int dmp_get_pedometer_walk_time(unsigned long *time)
{
unsigned char tmp[4];
if (!time)
return -1;
if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
return -1;
time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
return 0;
}
/**
* @brief Overwrite current walk time.
* WARNING: This function writes to DMP memory and could potentially encounter
* a race condition if called while the pedometer is enabled.
* @param[in] time New walk time in milliseconds.
*/
int dmp_set_pedometer_walk_time(unsigned long time)
{
unsigned char tmp[4];
time /= 20;
tmp[0] = (unsigned char)((time >> 24) & 0xFF);
tmp[1] = (unsigned char)((time >> 16) & 0xFF);
tmp[2] = (unsigned char)((time >> 8) & 0xFF);
tmp[3] = (unsigned char)(time & 0xFF);
return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
}
/**
* @brief Enable DMP features.
* The following \#define's are used in the input mask:
* \n DMP_FEATURE_TAP
* \n DMP_FEATURE_ANDROID_ORIENT
* \n DMP_FEATURE_LP_QUAT
* \n DMP_FEATURE_6X_LP_QUAT
* \n DMP_FEATURE_GYRO_CAL
* \n DMP_FEATURE_SEND_RAW_ACCEL
* \n DMP_FEATURE_SEND_RAW_GYRO
* \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
* exclusive.
* \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
* mutually exclusive.
* @param[in] mask Mask of features to enable.
* @return 0 if successful.
*/
int dmp_enable_feature(unsigned short mask)
{
unsigned char tmp[10];
/* TODO: All of these settings can probably be integrated into the default
* DMP image.
*/
/* Set integration scale factor. */
tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
mpu_write_mem(D_0_104, 4, tmp);
/* Send sensor data to the FIFO. */
tmp[0] = 0xA3;
if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
tmp[1] = 0xC0;
tmp[2] = 0xC8;
tmp[3] = 0xC2;
} else {
tmp[1] = 0xA3;
tmp[2] = 0xA3;
tmp[3] = 0xA3;
}
if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
tmp[4] = 0xC4;
tmp[5] = 0xCC;
tmp[6] = 0xC6;
} else {
tmp[4] = 0xA3;
tmp[5] = 0xA3;
tmp[6] = 0xA3;
}
tmp[7] = 0xA3;
tmp[8] = 0xA3;
tmp[9] = 0xA3;
mpu_write_mem(CFG_15,10,tmp);
/* Send gesture data to the FIFO. */
if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
tmp[0] = DINA20;
else
tmp[0] = 0xD8;
mpu_write_mem(CFG_27,1,tmp);
if (mask & DMP_FEATURE_GYRO_CAL)
dmp_enable_gyro_cal(1);
else
dmp_enable_gyro_cal(0);
if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
tmp[0] = 0xB2;
tmp[1] = 0x8B;
tmp[2] = 0xB6;
tmp[3] = 0x9B;
} else {
tmp[0] = DINAC0;
tmp[1] = DINA80;
tmp[2] = DINAC2;
tmp[3] = DINA90;
}
mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
}
if (mask & DMP_FEATURE_TAP) {
/* Enable tap. */
tmp[0] = 0xF8;
mpu_write_mem(CFG_20, 1, tmp);
dmp_set_tap_thresh(TAP_XYZ, 250);
dmp_set_tap_axes(TAP_XYZ);
dmp_set_tap_count(1);
dmp_set_tap_time(100);
dmp_set_tap_time_multi(500);
dmp_set_shake_reject_thresh(GYRO_SF, 200);
dmp_set_shake_reject_time(40);
dmp_set_shake_reject_timeout(10);
} else {
tmp[0] = 0xD8;
mpu_write_mem(CFG_20, 1, tmp);
}
if (mask & DMP_FEATURE_ANDROID_ORIENT) {
tmp[0] = 0xD9;
} else
tmp[0] = 0xD8;
mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
if (mask & DMP_FEATURE_LP_QUAT)
dmp_enable_lp_quat(1);
else
dmp_enable_lp_quat(0);
if (mask & DMP_FEATURE_6X_LP_QUAT)
dmp_enable_6x_lp_quat(1);
else
dmp_enable_6x_lp_quat(0);
/* Pedometer is always enabled. */
dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
mpu_reset_fifo();
dmp.packet_length = 0;
if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
dmp.packet_length += 6;
if (mask & DMP_FEATURE_SEND_ANY_GYRO)
dmp.packet_length += 6;
if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
dmp.packet_length += 16;
if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
dmp.packet_length += 4;
return 0;
}
/**
* @brief Get list of currently enabled DMP features.
* @param[out] Mask of enabled features.
* @return 0 if successful.
*/
int dmp_get_enabled_features(unsigned short *mask)
{
mask[0] = dmp.feature_mask;
return 0;
}
/**
* @brief Calibrate the gyro data in the DMP.
* After eight seconds of no motion, the DMP will compute gyro biases and
* subtract them from the quaternion output. If @e dmp_enable_feature is
* called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
* subtracted from the gyro output.
* @param[in] enable 1 to enable gyro calibration.
* @return 0 if successful.
*/
int dmp_enable_gyro_cal(unsigned char enable)
{
if (enable) {
unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
} else {
unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
}
}
/**
* @brief Generate 3-axis quaternions from the DMP.
* In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
* exclusive.
* @param[in] enable 1 to enable 3-axis quaternion.
* @return 0 if successful.
*/
int dmp_enable_lp_quat(unsigned char enable)
{
unsigned char regs[4];
if (enable) {
regs[0] = DINBC0;
regs[1] = DINBC2;
regs[2] = DINBC4;
regs[3] = DINBC6;
}
else
memset(regs, 0x8B, 4);
mpu_write_mem(CFG_LP_QUAT, 4, regs);
return mpu_reset_fifo();
}
/**
* @brief Generate 6-axis quaternions from the DMP.
* In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
* exclusive.
* @param[in] enable 1 to enable 6-axis quaternion.
* @return 0 if successful.
*/
int dmp_enable_6x_lp_quat(unsigned char enable)
{
unsigned char regs[4];
if (enable) {
regs[0] = DINA20;
regs[1] = DINA28;
regs[2] = DINA30;
regs[3] = DINA38;
} else
memset(regs, 0xA3, 4);
mpu_write_mem(CFG_8, 4, regs);
return mpu_reset_fifo();
}
/**
* @brief Decode the four-byte gesture data and execute any callbacks.
* @param[in] gesture Gesture data from DMP packet.
* @return 0 if successful.
*/
static int decode_gesture(unsigned char *gesture)
{
unsigned char tap, android_orient;
android_orient = gesture[3] & 0xC0;
tap = 0x3F & gesture[3];
if (gesture[1] & INT_SRC_TAP) {
unsigned char direction, count;
direction = tap >> 3;
count = (tap % 8) + 1;
if (dmp.tap_cb)
dmp.tap_cb(direction, count);
}
if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
if (dmp.android_orient_cb)
dmp.android_orient_cb(android_orient >> 6);
}
return 0;
}
/**
* @brief Specify when a DMP interrupt should occur.
* A DMP interrupt can be configured to trigger on either of the two
* conditions below:
* \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
* \n b. A tap event has been detected.
* @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
* @return 0 if successful.
*/
int dmp_set_interrupt_mode(unsigned char mode)
{
const unsigned char regs_continuous[11] =
{0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
const unsigned char regs_gesture[11] =
{0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
switch (mode) {
case DMP_INT_CONTINUOUS:
return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
(unsigned char*)regs_continuous);
case DMP_INT_GESTURE:
return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
(unsigned char*)regs_gesture);
default:
return -1;
}
}
/**
* @brief Get one packet from the FIFO.
* If @e sensors does not contain a particular sensor, disregard the data
* returned to that pointer.
* \n @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n INV_WXYZ_QUAT
* \n If the FIFO has no new data, @e sensors will be zero.
* \n If the FIFO is disabled, @e sensors will be zero and this function will
* return a non-zero error code.
* @param[out] gyro Gyro data in hardware units.
* @param[out] accel Accel data in hardware units.
* @param[out] quat 3-axis quaternion data in hardware units.
* @param[out] timestamp Timestamp in milliseconds.
* @param[out] sensors Mask of sensors read from FIFO.
* @param[out] more Number of remaining packets.
* @return 0 if successful.
*/
int dmp_read_fifo(short *gyro, short *accel, long *quat,
unsigned long *timestamp, short *sensors, unsigned char *more)
{
unsigned char fifo_data[MAX_PACKET_LENGTH];
unsigned char ii = 0;
/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
* cache this value and save some cycles.
*/
sensors[0] = 0;
/* Get a packet. */
if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
return -1;
/* Parse DMP packet. */
if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
#ifdef FIFO_CORRUPTION_CHECK
long quat_q14[4], quat_mag_sq;
#endif
quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
((long)fifo_data[2] << 8) | fifo_data[3];
quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
((long)fifo_data[6] << 8) | fifo_data[7];
quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
((long)fifo_data[10] << 8) | fifo_data[11];
quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
((long)fifo_data[14] << 8) | fifo_data[15];
ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
/* We can detect a corrupted FIFO by monitoring the quaternion data and
* ensuring that the magnitude is always normalized to one. This
* shouldn't happen in normal operation, but if an I2C error occurs,
* the FIFO reads might become misaligned.
*
* Let's start by scaling down the quaternion data to avoid long long
* math.
*/
quat_q14[0] = quat[0] >> 16;
quat_q14[1] = quat[1] >> 16;
quat_q14[2] = quat[2] >> 16;
quat_q14[3] = quat[3] >> 16;
quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
(quat_mag_sq > QUAT_MAG_SQ_MAX)) {
/* Quaternion is outside of the acceptable threshold. */
mpu_reset_fifo();
sensors[0] = 0;
return -1;
}
sensors[0] |= INV_WXYZ_QUAT;
#endif
}
if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_ACCEL;
}
if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_GYRO;
}
/* Gesture data is at the end of the DMP packet. Parse it and call
* the gesture callbacks (if registered).
*/
if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
decode_gesture(fifo_data + ii);
get_ms(timestamp);
return 0;
}
/**
* @brief Register a function to be executed on a tap event.
* The tap direction is represented by one of the following:
* \n TAP_X_UP
* \n TAP_X_DOWN
* \n TAP_Y_UP
* \n TAP_Y_DOWN
* \n TAP_Z_UP
* \n TAP_Z_DOWN
* @param[in] func Callback function.
* @return 0 if successful.
*/
int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
{
dmp.tap_cb = func;
return 0;
}
/**
* @brief Register a function to be executed on a android orientation event.
* @param[in] func Callback function.
* @return 0 if successful.
*/
int dmp_register_android_orient_cb(void (*func)(unsigned char))
{
dmp.android_orient_cb = func;
return 0;
}
/**
* @}
*/
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_ */
mpu6050.c
#include "stm32f1xx_hal.h"
#include "mpu6050.h"
//初始化MPU6050
//返回值:0,成功
// 其他,错误代码
uint8_t MPU_Init(void)
{
uint8_t res;
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(200); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(100); //设置采样率为50Hz
} else return 1;
return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_LPF(uint16_t lpf)
{
uint8_t data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Rate(uint16_t rate)
{
uint8_t data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
uint8_t buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((uint16_t)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
uint8_t buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((uint16_t)buf[0]<<8)|buf[1];
*gy=((uint16_t)buf[2]<<8)|buf[3];
*gz=((uint16_t)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
uint8_t buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((uint16_t)buf[0]<<8)|buf[1];
*ay=((uint16_t)buf[2]<<8)|buf[3];
*az=((uint16_t)buf[4]<<8)|buf[5];
}
return res;;
}
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
uint8_t i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
for(i=0; i<len; i++)
{
MPU_IIC_Send_Byte(buf[i]); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
MPU_IIC_Stop(); //产生一个停止条件
return 0;
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(data);//发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
uint8_t MPU_Read_Byte(uint8_t reg)
{
uint8_t res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
MPU_IIC_Stop(); //产生一个停止条件
return res;
}
mpu6050.h
#ifndef __MPU6050_H
#define __MPU6050_H
#include "IIC.h"
#define delay_ms HAL_Delay
#define MPU_IIC_Init IIC_GPIO_Init
#define MPU_IIC_Start IIC_Start
#define MPU_IIC_Stop IIC_Stop
#define MPU_IIC_Send_Byte IIC_Send_Byte
#define MPU_IIC_Read_Byte IIC_Read_Byte
#define MPU_IIC_Wait_Ack IIC_Wait_Ack
//#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 0XD0
uint8_t MPU_Init(void); //初始化MPU6050
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);//IIC连续写
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf); //IIC连续读
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data); //IIC写一个字节
uint8_t MPU_Read_Byte(uint8_t reg); //IIC读一个字节
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr);
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr);
uint8_t MPU_Set_LPF(uint16_t lpf);
uint8_t MPU_Set_Rate(uint16_t rate);
uint8_t MPU_Set_Fifo(uint8_t sens);
short MPU_Get_Temperature(void);
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az);
#endif
main.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2024 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "IIC.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "stdio.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
float pitch,roll,yaw;
uint8_t display_buf[20];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
/* USER CODE BEGIN 2 */
OLED_Init();
OLED_Clear();
MPU_Init();
mpu_dmp_init();
OLED_ShowString(0,00,"Init Sucess",16);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
HAL_Delay(10);
mpu_dmp_get_data(&pitch,&roll,&yaw);
sprintf((char *)display_buf,"pitch:%.2f ",pitch);
OLED_ShowString(0,2,display_buf,16);
sprintf((char *)display_buf,"roll:%.2f ",roll);
OLED_ShowString(0,4,display_buf,16);
sprintf((char *)display_buf,"yaw:%.2f ",yaw);
OLED_ShowString(0,6,display_buf,16);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
软件模拟iic
在软件模拟iic的时候一定要注意引脚是否能作为常规对的GPIO口引脚直接使用
平衡小车学习教程2(软件篇)——MPU6050数据读取欧拉角，移植DMP读取Roll角、Pitch角、Yaw角_平衡小车mpu6050-CSDN博客
IIC.h
#ifndef _IIC_H
#define _IIC_H
#include <inttypes.h>
#define IIC_WR 0 /* 写控制bit */
#define IIC_RD 1 /* 读控制bit */
void IIC_Start(void);
void IIC_Stop(void);
void IIC_Send_Byte(uint8_t _ucByte);
uint8_t IIC_Read_Byte(uint8_t ack);
uint8_t IIC_Wait_Ack(void);
void IIC_Ack(void);
void IIC_NAck(void);
uint8_t IIC_CheckDevice(uint8_t _Address);
void IIC_GPIO_Init(void);
#endif
IIC.c
#include "stm32f1xx_hal.h"
#include "IIC.h"
/* 定义IIC总线连接的GPIO端口, 用户只需要修改下面4行代码即可任意改变SCL和SDA的引脚 */
#define GPIO_PORT_IIC GPIOB /* GPIO端口 */
#define RCC_IIC_ENABLE __HAL_RCC_GPIOB_CLK_ENABLE() /* GPIO端口时钟 */
#define IIC_SCL_PIN GPIO_PIN_4 /* 连接到SCL时钟线的GPIO */
#define IIC_SDA_PIN GPIO_PIN_3 /* 连接到SDA数据线的GPIO */
/* 定义读写SCL和SDA的宏，已增加代码的可移植性和可阅读性 */
#if 1 /* 条件编译： 1 选择GPIO的库函数实现IO读写 */
#define IIC_SCL_1() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SCL_PIN, GPIO_PIN_SET) /* SCL = 1 */
#define IIC_SCL_0() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SCL_PIN, GPIO_PIN_RESET) /* SCL = 0 */
#define IIC_SDA_1() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SDA_PIN, GPIO_PIN_SET) /* SDA = 1 */
#define IIC_SDA_0() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SDA_PIN, GPIO_PIN_RESET) /* SDA = 0 */
#define IIC_SDA_READ() HAL_GPIO_ReadPin(GPIO_PORT_IIC, IIC_SDA_PIN) /* 读SDA口线状态 */
#else /* 这个分支选择直接寄存器操作实现IO读写 */
/* 注意：如下写法，在IAR最高级别优化时，会被编译器错误优化 */
#define IIC_SCL_1() GPIO_PORT_IIC->BSRR = IIC_SCL_PIN /* SCL = 1 */
#define IIC_SCL_0() GPIO_PORT_IIC->BRR = IIC_SCL_PIN /* SCL = 0 */
#define IIC_SDA_1() GPIO_PORT_IIC->BSRR = IIC_SDA_PIN /* SDA = 1 */
#define IIC_SDA_0() GPIO_PORT_IIC->BRR = IIC_SDA_PIN /* SDA = 0 */
#define IIC_SDA_READ() ((GPIO_PORT_IIC->IDR & IIC_SDA_PIN) != 0) /* 读SDA口线状态 */
#endif
void IIC_GPIO_Init(void);
/*
*********************************************************************************************************
* 函 数 名: IIC_Delay
* 功能说明: IIC总线位延迟，最快400KHz
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
static void IIC_Delay(void)
{
uint8_t i;
/*
下面的时间是通过安富莱AX-Pro逻辑分析仪测试得到的。
CPU主频72MHz时，在内部Flash运行, MDK工程不优化
循环次数为10时，SCL频率 = 205KHz
循环次数为7时，SCL频率 = 347KHz， SCL高电平时间1.5us，SCL低电平时间2.87us
循环次数为5时，SCL频率 = 421KHz， SCL高电平时间1.25us，SCL低电平时间2.375us
IAR工程编译效率高，不能设置为7
*/
for (i = 0; i < 10; i++);
}
/*
*********************************************************************************************************
* 函 数 名: IIC_Start
* 功能说明: CPU发起IIC总线启动信号
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_Start(void)
{
/* 当SCL高电平时，SDA出现一个下跳沿表示IIC总线启动信号 */
IIC_SDA_1();
IIC_SCL_1();
IIC_Delay();
IIC_SDA_0();
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
}
/*
*********************************************************************************************************
* 函 数 名: IIC_Start
* 功能说明: CPU发起IIC总线停止信号
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_Stop(void)
{
/* 当SCL高电平时，SDA出现一个上跳沿表示IIC总线停止信号 */
IIC_SDA_0();
IIC_SCL_1();
IIC_Delay();
IIC_SDA_1();
}
/*
*********************************************************************************************************
* 函 数 名: IIC_SendByte
* 功能说明: CPU向IIC总线设备发送8bit数据
* 形 参：_ucByte ： 等待发送的字节
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_Send_Byte(uint8_t _ucByte)
{
uint8_t i;
/* 先发送字节的高位bit7 */
for (i = 0; i < 8; i++)
{
if (_ucByte & 0x80)
{
IIC_SDA_1();
}
else
{
IIC_SDA_0();
}
IIC_Delay();
IIC_SCL_1();
IIC_Delay();
IIC_SCL_0();
if (i == 7)
{
IIC_SDA_1(); // 释放总线
}
_ucByte <<= 1; /* 左移一个bit */
IIC_Delay();
}
}
/*
*********************************************************************************************************
* 函 数 名: IIC_ReadByte
* 功能说明: CPU从IIC总线设备读取8bit数据
* 形 参：无
* 返 回 值: 读到的数据
*********************************************************************************************************
*/
uint8_t IIC_Read_Byte(uint8_t ack)
{
uint8_t i;
uint8_t value;
/* 读到第1个bit为数据的bit7 */
value = 0;
for (i = 0; i < 8; i++)
{
value <<= 1;
IIC_SCL_1();
IIC_Delay();
if (IIC_SDA_READ())
{
value++;
}
IIC_SCL_0();
IIC_Delay();
}
if(ack==0)
IIC_NAck();
else
IIC_Ack();
return value;
}
/*
*********************************************************************************************************
* 函 数 名: IIC_WaitAck
* 功能说明: CPU产生一个时钟，并读取器件的ACK应答信号
* 形 参：无
* 返 回 值: 返回0表示正确应答，1表示无器件响应
*********************************************************************************************************
*/
uint8_t IIC_Wait_Ack(void)
{
uint8_t re;
IIC_SDA_1(); /* CPU释放SDA总线 */
IIC_Delay();
IIC_SCL_1(); /* CPU驱动SCL = 1, 此时器件会返回ACK应答 */
IIC_Delay();
if (IIC_SDA_READ()) /* CPU读取SDA口线状态 */
{
re = 1;
}
else
{
re = 0;
}
IIC_SCL_0();
IIC_Delay();
return re;
}
/*
*********************************************************************************************************
* 函 数 名: IIC_Ack
* 功能说明: CPU产生一个ACK信号
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_Ack(void)
{
IIC_SDA_0(); /* CPU驱动SDA = 0 */
IIC_Delay();
IIC_SCL_1(); /* CPU产生1个时钟 */
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
IIC_SDA_1(); /* CPU释放SDA总线 */
}
/*
*********************************************************************************************************
* 函 数 名: IIC_NAck
* 功能说明: CPU产生1个NACK信号
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_NAck(void)
{
IIC_SDA_1(); /* CPU驱动SDA = 1 */
IIC_Delay();
IIC_SCL_1(); /* CPU产生1个时钟 */
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
}
/*
*********************************************************************************************************
* 函 数 名: IIC_GPIO_Config
* 功能说明: 配置IIC总线的GPIO，采用模拟IO的方式实现
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
void IIC_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_IIC_ENABLE; /* 打开GPIO时钟 */
GPIO_InitStructure.Pin = IIC_SCL_PIN | IIC_SDA_PIN;
GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_OD; /* 开漏输出 */
HAL_GPIO_Init(GPIO_PORT_IIC, &GPIO_InitStructure);
/* 给一个停止信号, 复位IIC总线上的所有设备到待机模式 */
IIC_Stop();
}
/*
*********************************************************************************************************
* 函 数 名: IIC_CheckDevice
* 功能说明: 检测IIC总线设备，CPU向发送设备地址，然后读取设备应答来判断该设备是否存在
* 形 参：_Address：设备的IIC总线地址
* 返 回 值: 返回值 0 表示正确， 返回1表示未探测到
*********************************************************************************************************
*/
uint8_t IIC_CheckDevice(uint8_t _Address)
{
uint8_t ucAck;
IIC_GPIO_Init(); /* 配置GPIO */
IIC_Start(); /* 发送启动信号 */
/* 发送设备地址+读写控制bit（0 = w， 1 = r) bit7 先传 */
IIC_Send_Byte(_Address|IIC_WR);
ucAck = IIC_Wait_Ack(); /* 检测设备的ACK应答 */
IIC_Stop(); /* 发送停止信号 */
return ucAck;
}
智能车逐飞库：
选自paper
滤波零漂效果非常好的，可以做惯导的水平
感兴趣的可以移植一下（后续也会测试效果）
卡尔曼滤波，四元素姿态解算
#include "mpu6050.h"
#include "mpuiic.h"
#include "delay.h"
#define delta_T 0.002f // 2ms计算一次 这里的时间可以改成5ms
#define alpha 0.3f
#define M_PI 3.1415926f
float GyroOffset_Xdata = 0, icm_data_acc_x = 0, icm_data_gyro_x = 0;
float GyroOffset_Ydata = 0, icm_data_acc_y = 0, icm_data_gyro_y = 0;
float GyroOffset_Zdata = 0, icm_data_acc_z = 0, icm_data_gyro_z = 0;
float Q_info_q0 = 1, Q_info_q1 = 0, Q_info_q2 = 0, Q_info_q3 = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
float eulerAngle_yaw = 0, eulerAngle_pitch = 0, eulerAngle_roll = 0, eulerAngle_yaw_total = 0, eulerAngle_yaw_old = 0;
float I_ex, I_ey, I_ez; // 误差积分
float test = 1;
float test1 = 1;
float test2 = 1;
float test3 = 1;
float fast_sqrt(float num)
{
float halfx = 0.5f * num;
float y = num;
long i = *(long*)&y;
i = 0x5f375a86 - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
// float y = sqrtf(num);
// return y;
}
void gyroOffset_init(void)
{ /陀螺仪零飘初始化
GyroOffset_Xdata = 0;
GyroOffset_Ydata = 0;
GyroOffset_Zdata = 0;
// for (uint16_t i = 0; i < 5000; i++) { ///这里这段是计5000次上下摆幅的数据记录零漂
// MPU_Get_Gyroscope( );
// GyroOffset_Xdata += mpu6050_gyro_x;
// GyroOffset_Ydata += mpu6050_gyro_y;
// GyroOffset_Zdata += mpu6050_gyro_z;
// HAL_Delay(5);//这里的单位是毫秒
// }
// GyroOffset_Xdata /= 5000;
// GyroOffset_Ydata /= 5000;
// GyroOffset_Zdata /= 5000;
GyroOffset_Xdata = 0.2511;
GyroOffset_Ydata = -1.1646;
GyroOffset_Zdata = 1.1402;
}
// 转化为实际物理值
void ICM_getValues()
{
// 一阶低通滤波，单位g/s
icm_data_acc_x = (((float)mpu6050_acc_x) * alpha) + icm_data_acc_x * (1 - alpha);
icm_data_acc_y = (((float)mpu6050_acc_y) * alpha) + icm_data_acc_y * (1 - alpha);
icm_data_acc_z = (((float)mpu6050_acc_z) * alpha) + icm_data_acc_z * (1 - alpha);
// 陀螺仪角速度转弧度
icm_data_gyro_x = ((float)mpu6050_gyro_x - GyroOffset_Xdata) * M_PI / 180 / 16.4f;
icm_data_gyro_y = ((float)mpu6050_gyro_y - GyroOffset_Ydata) * M_PI / 180 / 16.4f;
icm_data_gyro_z = ((float)mpu6050_gyro_z - GyroOffset_Zdata) * M_PI / 180 / 16.4f;
}
// 互补滤波
void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float halfT = 0.5 * delta_T;
float vx, vy, vz; // 当前的机体坐标系上的重力单位向量
float ex, ey, ez; // 四元数计算值与加速度计测量值的误差
float q0 = Q_info_q0;
float q1 = Q_info_q1;
float q2 = Q_info_q2;
float q3 = Q_info_q3;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
//float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
//float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
// float delta_2 = 0;
// 对加速度数据进行归一化 得到单位加速度
float norm = fast_sqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
// 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
// vz = (q0*q0-0.5f+q3 * q3) * 2;
// 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
// 用叉乘误差来做PI修正陀螺零偏，
// 通过调节 param_Kp，param_Ki 两个参数，
// 可以控制加速度计修正陀螺仪积分姿态的速度。
I_ex += halfT * ex; // integral error scaled by Ki
I_ey += halfT * ey;
I_ez += halfT * ez;
gx = gx + param_Kp * ex + param_Ki * I_ex;
gy = gy + param_Kp * ey + param_Ki * I_ey;
gz = gz + param_Kp * ez + param_Ki * I_ez;
/*数据修正完成，下面是四元数微分方程*/
// 四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
// delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
// 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法
// q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
// q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
// q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
// q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT
// normalise quaternion
norm = fast_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Q_info_q0 = q0 * norm;
Q_info_q1 = q1 * norm;
Q_info_q2 = q2 * norm;
Q_info_q3 = q3 * norm;
}
// 获取车辆姿态
void ICM_getEulerianAngles(void)
{
// 采集陀螺仪数据
MPU_Get_Gyroscope();
MPU_Get_Accelerometer();
mpu6050_acc_x = mpu6050_acc_transition(mpu6050_acc_x);
mpu6050_acc_y = mpu6050_acc_transition(mpu6050_acc_y);
mpu6050_acc_z = mpu6050_acc_transition(mpu6050_acc_z);
ICM_getValues();
ICM_AHRSupdate(icm_data_gyro_x, icm_data_gyro_y, icm_data_gyro_z, icm_data_acc_x, icm_data_acc_y, icm_data_acc_z);
// ICM_AHRSupdate(icm_data_gyro_y, icm_data_gyro_z, icm_data_gyro_x, icm_data_acc_y, icm_data_acc_z, icm_data_acc_x);//修改陀螺仪位置后，改这里，姿态解算就不会出错啦
// ZX XY YZ
// xy yz zx
float q0 = Q_info_q0;
float q1 = Q_info_q1;
float q2 = Q_info_q2;
float q3 = Q_info_q3;
// 四元数计算欧拉角---原始
eulerAngle_roll = -asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch
eulerAngle_pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
eulerAngle_yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw
float i = eulerAngle_yaw - eulerAngle_yaw_old;
if (i < -180) {
i += 360;
}
else if (i > 180) {
i -= 360;
}
eulerAngle_yaw_total += i;
eulerAngle_yaw_old = eulerAngle_yaw;
//姿态限制
// if (eulerAngle_yaw > 360) {
// eulerAngle_yaw -= 360;
// }
// else if (eulerAngle_yaw > 0) {
// eulerAngle_yaw += 360;
// }
}
/*
* init.h
*
* Created on: 2022年12月16日
* Author: paper
*/
#ifndef CODE_IMU_H_
#define CODE_IMU_H_
#include "zf_common_headfile.h"
extern float GyroOffset_Xdata;
extern float GyroOffset_Ydata;
extern float GyroOffset_Zdata;
extern float GyroOffset_Xdata, icm_data_acc_x, icm_data_gyro_x;
extern float GyroOffset_Ydata, icm_data_acc_y, icm_data_gyro_y;
extern float GyroOffset_Zdata, icm_data_acc_z, icm_data_gyro_z;
void gyroOffset_init(void);
void ICM_getEulerianAngles(void);
extern float eulerAngle_yaw, eulerAngle_pitch, eulerAngle_roll, eulerAngle_yaw_total, test,test1,test2,test3;
#endif /* CODE_IMU_H_ */逐飞mpu6050底层库：
/*********************************************************************************************************************
* TC264 Opensourec Library 即（TC264 开源库）是一个基于官方 SDK 接口的第三方开源库
* Copyright (c) 2022 SEEKFREE 逐飞科技
*
* 本文件是 TC264 开源库的一部分
*
* TC264 开源库 是免费软件
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
*
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
* 甚至没有隐含的适销性或适合特定用途的保证
* 更多细节请参见 GPL
*
* 您应该在收到本开源库的同时收到一份 GPL 的副本
* 如果没有，请参阅<https://www.gnu.org/licenses/>
*
* 额外注明：
* 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本
* 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中
* 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件
* 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明）
*
* 文件名称 zf_device_mpu6050
* 公司名称 成都逐飞科技有限公司
* 版本信息 查看 libraries/doc 文件夹内 version 文件 版本说明
* 开发环境 ADS v1.8.0
* 适用平台 TC264D
* 店铺链接 https://seekfree.taobao.com/
*
* 修改记录
* 日期 作者 备注
* 2022-09-15 pudding first version
********************************************************************************************************************/
/*********************************************************************************************************************
* 接线定义：
* ------------------------------------
* 模块管脚 单片机管脚
* 软件 IIC 通信引脚对应关系
* SCL 查看 zf_device_mpu6050.h 中 MPU6050_SOFT_IIC_SCL 宏定义
* SDA 查看 zf_device_mpu6050.h 中 MPU6050_SOFT_IIC_SDA 宏定义
* VCC 3.3V电源
* GND 电源地
* 其余引脚悬空
*
* 硬件 IIC 通信引脚应关系
* SCL 查看 zf_device_mpu6050.h 中 MPU6050_IIC_SCL 宏定义
* SDA 查看 zf_device_mpu6050.h 中 MPU6050_IIC_SDA 宏定义
* VCC 3.3V电源
* GND 电源地
* 其余引脚悬空
* ------------------------------------
********************************************************************************************************************/
#include "zf_common_debug.h"
#include "zf_driver_delay.h"
#include "zf_driver_soft_iic.h"
#include "zf_device_mpu6050.h"
int16 mpu6050_gyro_x = 0, mpu6050_gyro_y = 0, mpu6050_gyro_z = 0; // 三轴陀螺仪数据 gyro (陀螺仪)
int16 mpu6050_acc_x = 0, mpu6050_acc_y = 0, mpu6050_acc_z = 0; // 三轴加速度计数据 acc (accelerometer 加速度计)
#if MPU6050_USE_SOFT_IIC
static soft_iic_info_struct mpu6050_iic_struct;
#define mpu6050_write_register(reg, data) (soft_iic_write_8bit_register(&mpu6050_iic_struct, (reg), (data)))
#define mpu6050_read_register(reg) (soft_iic_read_8bit_register(&mpu6050_iic_struct, (reg)))
#define mpu6050_read_registers(reg, data, len) (soft_iic_read_8bit_registers(&mpu6050_iic_struct, (reg), (data), (len)))
#endif
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 MPU6050 自检
// 参数说明 void
// 返回参数 uint8 1-自检失败 0-自检成功
// 使用示例 if(mpu6050_self1_check())
// 备注信息 内部调用
//-------------------------------------------------------------------------------------------------------------------
static uint8 mpu6050_self1_check (void)
{
uint8 dat = 0, return_state = 0;
uint16 timeout_count = 0;
mpu6050_write_register(MPU6050_PWR_MGMT_1, 0x00); // 解除休眠状态
mpu6050_write_register(MPU6050_SMPLRT_DIV, 0x07); // 125HZ采样率
while(0x07 != dat)
{
if(timeout_count ++ > MPU6050_TIMEOUT_COUNT)
{
return_state = 1;
break;
}
dat = mpu6050_read_register(MPU6050_SMPLRT_DIV);
system_delay_ms(10);
}
return return_state;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 获取 MPU6050 加速度计数据
// 参数说明 void
// 返回参数 void
// 使用示例 mpu6050_get_acc(); // 执行该函数后，直接查看对应的变量即可
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_acc (void)
{
uint8 dat[6];
mpu6050_read_registers(MPU6050_ACCEL_XOUT_H, dat, 6);
mpu6050_acc_x = (int16)(((uint16)dat[0] << 8 | dat[1]));
mpu6050_acc_y = (int16)(((uint16)dat[2] << 8 | dat[3]));
mpu6050_acc_z = (int16)(((uint16)dat[4] << 8 | dat[5]));
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 获取 MPU6050 陀螺仪数据
// 参数说明 void
// 返回参数 void
// 使用示例 mpu6050_get_gyro(); // 执行该函数后，直接查看对应的变量即可
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_gyro (void)
{
uint8 dat[6];
mpu6050_read_registers(MPU6050_GYRO_XOUT_H, dat, 6);
mpu6050_gyro_x = (int16)(((uint16)dat[0] << 8 | dat[1]));
mpu6050_gyro_y = (int16)(((uint16)dat[2] << 8 | dat[3]));
mpu6050_gyro_z = (int16)(((uint16)dat[4] << 8 | dat[5]));
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 将 MPU6050 加速度计数据转换为实际物理数据
// 参数说明 gyro_value // 任意轴的加速度计数据
// 返回参数 void
// 使用示例 float data = mpu6050_acc_transition(imu660ra_acc_x); //单位为 g(m/s^2)
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float mpu6050_acc_transition (int16 acc_value)
{
float acc_data = 0;
switch(MPU6050_ACC_SAMPLE)
{
case 0x00: acc_data = (float)acc_value / 16384; break; // 0x00 加速度计量程为:±2g 获取到的加速度计数据 除以16384 可以转化为带物理单位的数据，单位：g(m/s^2)
case 0x08: acc_data = (float)acc_value / 8192; break; // 0x08 加速度计量程为:±4g 获取到的加速度计数据 除以8192 可以转化为带物理单位的数据，单位：g(m/s^2)
case 0x10: acc_data = (float)acc_value / 4096; break; // 0x10 加速度计量程为:±8g 获取到的加速度计数据 除以4096 可以转化为带物理单位的数据，单位：g(m/s^2)
case 0x18: acc_data = (float)acc_value / 2048; break; // 0x18 加速度计量程为:±16g 获取到的加速度计数据 除以2048 可以转化为带物理单位的数据，单位：g(m/s^2)
default: break;
}
return acc_data;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 将 MPU6050 陀螺仪数据转换为实际物理数据
// 参数说明 gyro_value // 任意轴的陀螺仪数据
// 返回参数 void
// 使用示例 float data = mpu6050_gyro_transition(imu660ra_gyro_x); // 单位为°/s
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float mpu6050_gyro_transition (int16 gyro_value)
{
float gyro_data = 0;
switch(MPU6050_GYR_SAMPLE)
{
case 0x00: gyro_data = (float)gyro_value / 131.2f; break; // 0x00 陀螺仪量程为:±250 dps 获取到的陀螺仪数据除以131 可以转化为带物理单位的数据，单位为：°/s
case 0x08: gyro_data = (float)gyro_value / 65.6f; break; // 0x08 陀螺仪量程为:±500 dps 获取到的陀螺仪数据除以65.5 可以转化为带物理单位的数据，单位为：°/s
case 0x10: gyro_data = (float)gyro_value / 32.8f; break; // 0x10 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以32.8 可以转化为带物理单位的数据，单位为：°/s
case 0x18: gyro_data = (float)gyro_value / 16.4f; break; // 0x18 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以16.4 可以转化为带物理单位的数据，单位为：°/s
default: break;
}
return gyro_data;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介 初始化 MPU6050
// 参数说明 void
// 返回参数 uint8 1-初始化失败 0-初始化成功
// 使用示例 mpu6050_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
uint8 mpu6050_init (void)
{
uint8 return_state = 0;
#if MPU6050_USE_SOFT_IIC
soft_iic_init(&mpu6050_iic_struct, MPU6050_DEV_ADDR, MPU6050_SOFT_IIC_DELAY, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#else
iic_init(MPU6050_IIC, MPU6050_DEV_ADDR, MPU6050_IIC_SPEED, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#endif
system_delay_ms(100); // 上电延时
do
{
if(mpu6050_self1_check())
{
// 如果程序在输出了断言信息 并且提示出错位置在这里
// 那么就是 MPU6050 自检出错并超时退出了
// 检查一下接线有没有问题 如果没问题可能就是坏了
zf_log(0, "MPU6050 self check error.");
return_state = 1;
break;
}
mpu6050_write_register(MPU6050_PWR_MGMT_1, 0x00); // 解除休眠状态
mpu6050_write_register(MPU6050_SMPLRT_DIV, 0x07); // 125HZ采样率
mpu6050_write_register(MPU6050_CONFIG, 0x04);
mpu6050_write_register(MPU6050_GYRO_CONFIG, MPU6050_GYR_SAMPLE); // 2000°/s
mpu6050_write_register(MPU6050_ACCEL_CONFIG, MPU6050_ACC_SAMPLE); // 8g(m/s^2)
mpu6050_write_register(MPU6050_USER_CONTROL, 0x00);
mpu6050_write_register(MPU6050_INT_PIN_CFG, 0x02);
// MPU6050_GYRO_CONFIG寄存器
// 设置为:0x00 陀螺仪量程为:±250 dps 获取到的陀螺仪数据除以131.2 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x08 陀螺仪量程为:±500 dps 获取到的陀螺仪数据除以65.6 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x10 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以32.8 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x18 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以16.4 可以转化为带物理单位的数据，单位为：°/s
// MPU6050_ACCEL_CONFIG寄存器
// 设置为:0x00 加速度计量程为:±2g 获取到的加速度计数据 除以16384 可以转化为带物理单位的数据，单位：g(m/s^2)
// 设置为:0x08 加速度计量程为:±4g 获取到的加速度计数据 除以8192 可以转化为带物理单位的数据，单位：g(m/s^2)
// 设置为:0x10 加速度计量程为:±8g 获取到的加速度计数据 除以4096 可以转化为带物理单位的数据，单位：g(m/s^2)
// 设置为:0x18 加速度计量程为:±16g 获取到的加速度计数据 除以2048 可以转化为带物理单位的数据，单位：g(m/s^2)
}while(0);
return return_state;
}/*********************************************************************************************************************
* TC264 Opensourec Library 即（TC264 开源库）是一个基于官方 SDK 接口的第三方开源库
* Copyright (c) 2022 SEEKFREE 逐飞科技
*
* 本文件是 TC264 开源库的一部分
*
* TC264 开源库 是免费软件
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
*
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
* 甚至没有隐含的适销性或适合特定用途的保证
* 更多细节请参见 GPL
*
* 您应该在收到本开源库的同时收到一份 GPL 的副本
* 如果没有，请参阅<https://www.gnu.org/licenses/>
*
* 额外注明：
* 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本
* 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中
* 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件
* 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明）
*
* 文件名称 zf_device_mpu6050
* 公司名称 成都逐飞科技有限公司
* 版本信息 查看 libraries/doc 文件夹内 version 文件 版本说明
* 开发环境 ADS v1.8.0
* 适用平台 TC264D
* 店铺链接 https://seekfree.taobao.com/
*
* 修改记录
* 日期 作者 备注
* 2022-09-15 pudding first version
********************************************************************************************************************/
/*********************************************************************************************************************
* 接线定义：
* ------------------------------------
* 模块管脚 单片机管脚
* 软件 IIC 通信引脚对应关系
* SCL 查看 zf_device_mpu6050.h 中 MPU6050_SOFT_IIC_SCL 宏定义
* SDA 查看 zf_device_mpu6050.h 中 MPU6050_SOFT_IIC_SDA 宏定义
* VCC 3.3V电源
* GND 电源地
* 其余引脚悬空
*
* 硬件 IIC 通信引脚应关系
* SCL 查看 zf_device_mpu6050.h 中 MPU6050_IIC_SCL 宏定义
* SDA 查看 zf_device_mpu6050.h 中 MPU6050_IIC_SDA 宏定义
* VCC 3.3V电源
* GND 电源地
* 其余引脚悬空
* ------------------------------------
********************************************************************************************************************/
#ifndef _zf_device_mpu6050_h_
#define _zf_device_mpu6050_h_
#include "zf_common_typedef.h"
#define MPU6050_USE_SOFT_IIC (1) // 默认使用软件 IIC 方式驱动 建议使用软件 IIC 方式
#if MPU6050_USE_SOFT_IIC // 这两段 颜色正常的才是正确的 颜色灰的就是没有用的
//====================================================软件 IIC 驱动====================================================
#define MPU6050_SOFT_IIC_DELAY (59 ) // 软件 IIC 的时钟延时周期 数值越小 IIC 通信速率越快
#define MPU6050_SCL_PIN (P20_11) // 软件 IIC SCL 引脚 连接 MPU6050 的 SCL 引脚
#define MPU6050_SDA_PIN (P20_14) // 软件 IIC SDA 引脚 连接 MPU6050 的 SDA 引脚
//====================================================软件 IIC 驱动====================================================
#endif
#define MPU6050_TIMEOUT_COUNT (0x00FF) // MPU6050 超时计数
//================================================定义 MPU6050 内部地址================================================
#define MPU6050_DEV_ADDR (0xD0>>1) // IIC写入时的地址字节数据，+1为读取
#define MPU6050_SMPLRT_DIV (0x19) // 陀螺仪采样率，典型值：0x07(125Hz)
#define MPU6050_CONFIG (0x1A) // 低通滤波频率，典型值：0x06(5Hz)
#define MPU6050_GYRO_CONFIG (0x1B) // 陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define MPU6050_ACCEL_CONFIG (0x1C) // 加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define MPU6050_INT_PIN_CFG (0x37) // 设置6050辅助I2C为直通模式寄存器
#define MPU6050_ACCEL_XOUT_H (0x3B)
#define MPU6050_GYRO_XOUT_H (0x43)
#define MPU6050_USER_CONTROL (0x6A) // 关闭6050对辅助I2C设备的控制
#define MPU6050_PWR_MGMT_1 (0x6B) // 电源管理，典型值：0x00(正常启用)
#define MPU6050_WHO_AM_I (0x75) // IIC地址寄存器(默认数值0x68，只读)
#define MPU6050_ACC_SAMPLE (0x10) // 加速度计量程
// 设置为:0x00 陀螺仪量程为:±250 dps 获取到的陀螺仪数据除以131.2 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x08 陀螺仪量程为:±500 dps 获取到的陀螺仪数据除以65.6 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x10 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以32.8 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x18 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以16.4 可以转化为带物理单位的数据，单位为：°/s
#define MPU6050_GYR_SAMPLE (0x18) // 陀螺仪量程
// 设置为:0x00 陀螺仪量程为:±250 dps 获取到的陀螺仪数据除以131.2 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x08 陀螺仪量程为:±500 dps 获取到的陀螺仪数据除以65.6 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x10 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以32.8 可以转化为带物理单位的数据，单位为：°/s
// 设置为:0x18 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以16.4 可以转化为带物理单位的数据，单位为：°/s
//================================================定义 MPU6050 内部地址================================================
//================================================声明 MPU6050 数据存储变量==============================================
extern int16 mpu6050_gyro_x, mpu6050_gyro_y, mpu6050_gyro_z; // 三轴陀螺仪数据 gyro (陀螺仪)
extern int16 mpu6050_acc_x, mpu6050_acc_y, mpu6050_acc_z; // 三轴加速度计数据 acc (accelerometer 加速度计)
//================================================声明 MPU6050 数据存储变量==============================================
//===================================================MPU6050 基础函数=================================================
void mpu6050_get_acc (void); // 获取 MPU6050 加速度计数据
void mpu6050_get_gyro (void); // 获取 MPU6050 陀螺仪数据
float mpu6050_acc_transition (int16 acc_value); // 将 MPU6050 加速度计数据转换为实际物理数据
float mpu6050_gyro_transition (int16 gyro_value); // 将 MPU6050 陀螺仪数据转换为实际物理数据
uint8 mpu6050_init (void); // 初始化 MPU6050
//===================================================MPU6050 基础函数=================================================
#endif作者：Plainper
