STM32标准库HAL库——MPU6050原理和代码
目录
陀螺仪相关基础知识：
加速度计，陀螺仪的工作原理：
陀螺仪再智能车中的应用：
MPU6050原理图和封装图：
硬件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.c
智能车逐飞库：
陀螺仪相关基础知识：
陀螺仪是感受空间姿态的传感器，是控制小车平衡，判断和调节姿态的核心元件
六轴陀螺仪结合了三轴陀螺仪和三轴加速度计，其“六轴”分别为加速度xyz轴，角速度xyz轴，即既能感知角度变化，也能感知加速度变化，这六个量经过运算，可以返回三个姿态角：
俯仰角（车头绕Y轴翘起角度），航向角（车身绕Z轴旋转角度），翻滚角（侧轮绕X轴抬起角度）。
（九轴多了三轴磁力计，需要XCL和XDA，本文不讲述和研究相关内容）
加速度计，陀螺仪的工作原理：
干活文章但较难：[陀螺仪底层到应用]-------学习草稿_陀螺仪数据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和软件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高电平期间读取SDAMyI2C_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高电平期间读取SDAif (MyI2C_R_SDA() == 1){Byte |= (0x80 >> i);} //读取SDA数据，并存储到Byte变量//当SDA为1时，置变量指定位为1，当SDA为0时，不做处理，指定位为默认的初值0MyI2C_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高电平期间读取SDAAckBit = MyI2C_R_SDA(); //将应答位存储到变量里MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块return AckBit; //返回定义应答位变量
}
MyI2C.h
#ifndef __MYI2C_H
#define __MYI2C_Hvoid 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); //配置寄存器，配置DLPFMPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); //陀螺仪配置寄存器，选择满量程为±2000°/sMPU6050_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=GX/16384;*T_ACCY=GY/16384;*T_ACCY=GZ/16384;
}
MPU6050.h
#ifndef __MPU6050_H
#define __MPU6050_Hvoid 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 ConfigurationPB10 ------> I2C1_SCLPB11 ------> 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 ConfigurationPB6 ------> I2C1_SCLPB7 ------> 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)
{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); //配置寄存器，配置DLPFMPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); //陀螺仪配置寄存器，选择满量程为±2000°/sMPU6050_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=GX/16384;*T_ACCY=GY/16384;*T_ACCY=GZ/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高电平期间读取SDAMyI2C_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高电平期间读取SDAif (MyI2C_R_SDA() == 1){Byte |= (0x80 >> i);} //读取SDA数据，并存储到Byte变量//当SDA为1时，置变量指定位为1，当SDA为0时，不做处理，指定位为默认的初值0MyI2C_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高电平期间读取SDAAckBit = MyI2C_R_SDA(); //将应答位存储到变量里MyI2C_W_SCL(0); //拉低SCL，开始下一个时序模块return AckBit; //返回定义应答位变量
}
智能车逐飞库：
选自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.1415926ffloat 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/sicm_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 KiI_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 quaternionnorm = 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 zxfloat 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; // pitcheulerAngle_pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // rolleulerAngle_yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yawfloat 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 可以转化为带物理单位的数据，单位为：°/scase 0x08: gyro_data = (float)gyro_value / 65.6f; break; // 0x08 陀螺仪量程为:±500 dps 获取到的陀螺仪数据除以65.5 可以转化为带物理单位的数据，单位为：°/scase 0x10: gyro_data = (float)gyro_value / 32.8f; break; // 0x10 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以32.8 可以转化为带物理单位的数据，单位为：°/scase 0x18: gyro_data = (float)gyro_value / 16.4f; break; // 0x18 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以16.4 可以转化为带物理单位的数据，单位为：°/sdefault: break;}return gyro_data;
}//-------------------------------------------------------------------------------------------------------------------
// 函数简介 初始化 MPU6050
// 参数说明 void
// 返回参数 uint8 1-初始化失败 0-初始化成功
// 使用示例 mpu6050_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
uint8 mpu6050_init (void)
{uint8 return_state = 0;
#if MPU6050_USE_SOFT_IICsoft_iic_init(&mpu6050_iic_struct, MPU6050_DEV_ADDR, MPU6050_SOFT_IIC_DELAY, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#elseiic_init(MPU6050_IIC, MPU6050_DEV_ADDR, MPU6050_IIC_SPEED, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#endifsystem_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°/smpu6050_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