【STM32】玩转IIC之驱动MPU6050及姿态解算

寄存器地址	功能描述
0x6B	PWR_MGMT_1：电源管理寄存器，用于设备复位和时钟源设置。
0x6C	PWR_MGMT_2：电源管理寄存器，用于设备睡眠模式控制。
0x1B	GYRO_CONFIG：陀螺仪配置寄存器，用于设置陀螺仪的量程范围。
0x1C	ACCEL_CONFIG：加速度计配置寄存器，用于设置加速度计的量程范围。
0x1A	CONFIG：配置寄存器，用于设置数字低通滤波器（DLPF）。
0x19	SMPLRT_DIV：采样率分频寄存器，用于设置数据采样率。
0x3B-0x40	加速度计和陀螺仪数据寄存器，分别存储加速度和角速度的原始数据。

通过配置这些寄存器，可以实现对MPU6050的初始化和数据读取。

二.MPU6050驱动开发

2.1 配置寄存器

配置寄存器用于设置MPU6050的工作模式、采样率、滤波器等参数。以下是几个常用配置寄存器的详细说明：

PWR_MGMT_1（0x6B）：
- 位7 ：设备复位位，写入0x80复位设备。
- 位0-2 ：时钟源选择，通常设置为0x00，使用内部时钟。
GYRO_CONFIG（0x1B）：
- 位3-0：陀螺仪量程选择，可选范围为±250°/s、±500°/s、±1000°/s、±2000°/s。
ACCEL_CONFIG（0x1C）：
- 位3-0：加速度计量程选择，可选范围为±2g、±4g、±8g、±16g。
SMPLRT_DIV（0x19）：
- 位7-0：采样率分频值，设置为0时，采样率最高。

对于寄存器这里我另外写了一个头文件包含了对MPU6050所有的寄存器地址进行声明，方便调用，简单明了！

复制代码

#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
 
#endif

2.2 对MPU6050寄存器进行读写

2.2.1 写入寄存器

通过IIC接口向MPU6050的寄存器写入数据，可以配置传感器的工作模式、量程、采样率等参数。以下是写入寄存器的代码实现：

复制代码

#define MPU6050_Addr 0xD0

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data) {
    IIC_Start();
    IIC_SendByte(MPU6050_Addr); // 发送设备地址（写操作）
    IIC_ReceiveAck();
    IIC_SendByte(RegAddress); // 发送寄存器地址
    IIC_ReceiveAck();
    IIC_SendByte(Data); // 发送数据
    IIC_ReceiveAck();
    IIC_Stop();
}

2.2.2读取寄存器

通过IIC接口从MPU6050的寄存器读取数据，可以获取传感器的配置状态或测量数据。以下是读取寄存器的代码实现：

复制代码

uint8_t MPU6050_ReadReg(uint8_t RegAddress) {
    uint8_t Data;
    IIC_Start();
    IIC_SendByte(MPU6050_Addr); // 发送设备地址（写操作）
    IIC_ReceiveAck();
    IIC_SendByte(RegAddress); // 发送寄存器地址
    IIC_ReceiveAck();

    IIC_Start();
    IIC_SendByte(MPU6050_Addr | 0x01); // 发送设备地址（读操作）
    IIC_ReceiveAck();
    Data = IIC_ReceiveByte(); // 读取数据
    IIC_SendAck(1); // 发送NACK，结束读取
    IIC_Stop();

    return Data;
}

2.3 初始化MPU6050

2.3.1 设置工作模式

通过PWR_MGMT_1寄存器设置MPU6050的工作模式。通常在初始化时，先复位设备，然后将其设置为正常工作模式。

复制代码

void MPU6050_SetPowerMode(uint8_t mode) 
{
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, mode);
}

2.3.2 配置采样率

通过SMPLRT_DIV寄存器设置采样率分频值，从而控制数据的采样频率。

复制代码

void MPU6050_SetSampleRate(uint8_t div) 
{
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, div);
}

2.3.3 启用传感器

通过PWR_MGMT_1寄存器启用加速度计和陀螺仪。

复制代码

void MPU6050_EnableSensors(void) 
{
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 启用设备
}

2.4MPU6050数据读取

2.4.1 读取加速度数据

加速度数据存储在寄存器0x3B到0x40中，分别表示加速度计的X、Y、Z轴数据。

复制代码

void MPU6050_ReadAccel(int16_t *ax, int16_t *ay, int16_t *az) 
{
    uint8_t buffer[6]; // 创建一个6字节的缓冲区，用于存储从MPU6050读取的加速度数据

    // 逐个读取加速度计的X、Y、Z轴的高字节和低字节数据
    buffer[0] = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); // X轴加速度数据的高字节
    buffer[1] = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); // X轴加速度数据的低字节
    buffer[2] = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); // Y轴加速度数据的高字节
    buffer[3] = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); // Y轴加速度数据的低字节
    buffer[4] = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); // Z轴加速度数据的高字节
    buffer[5] = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); // Z轴加速度数据的低字节

    // 将高字节和低字节组合成完整的16位加速度数据
    *ax = (buffer[0] << 8) | buffer[1]; // 将X轴的高字节左移8位后与低字节进行按位或操作，得到完整的X轴加速度数据
    *ay = (buffer[2] << 8) | buffer[3]; // 同理，得到Y轴加速度数据
    *az = (buffer[4] << 8) | buffer[5]; // 同理，得到Z轴加速度数据
}

2.4.2 读取陀螺仪数据

陀螺仪数据存储在寄存器0x43到0x48中，分别表示陀螺仪的X、Y、Z轴数据。

复制代码

void MPU6050_ReadGyro(int16_t *gx, int16_t *gy, int16_t *gz) 
{
    uint8_t buffer[6]; // 创建一个6字节的缓冲区，用于存储从MPU6050读取的陀螺仪数据

    // 逐个读取陀螺仪的X、Y、Z轴的高字节和低字节数据
    buffer[0] = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); // X轴陀螺仪数据的高字节
    buffer[1] = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); // X轴陀螺仪数据的低字节
    buffer[2] = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); // Y轴陀螺仪数据的高字节
    buffer[3] = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); // Y轴陀螺仪数据的低字节
    buffer[4] = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); // Z轴陀螺仪数据的高字节
    buffer[5] = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); // Z轴陀螺仪数据的低字节

    // 将高字节和低字节组合成完整的16位陀螺仪数据
    *gx = (buffer[0] << 8) | buffer[1]; // 将X轴的高字节左移8位后与低字节进行按位或操作，得到完整的X轴陀螺仪数据
    *gy = (buffer[2] << 8) | buffer[3]; // 同理，得到Y轴陀螺仪数据
    *gz = (buffer[4] << 8) | buffer[5]; // 同理，得到Z轴陀螺仪数据
}

2.4.3 数据格式转换

将读取到的原始数据转换为实际的物理量。

复制代码

float MPU6050_ConvertAccel(int16_t value, uint8_t range) 
{
    float factor; // 用于存储转换因子
    // 根据加速度计的量程选择合适的转换因子
    switch (range) {
        case 0x00: factor = 16384.0; break; // ±2g，转换因子为16384
        case 0x08: factor = 8192.0; break;  // ±4g，转换因子为8192
        case 0x10: factor = 4096.0; break;  // ±8g，转换因子为4096
        case 0x18: factor = 2048.0; break;  // ±16g，转换因子为2048
        default: factor = 16384.0; break;    // 默认量程范围为±2g
    }
    // 将原始加速度值转换为g单位
    return value / factor;
}

float MPU6050_ConvertGyro(int16_t value, uint8_t range) 
{
    float factor; // 用于存储转换因子
    // 根据陀螺仪的量程选择合适的转换因子
    switch (range) {
        case 0x00: factor = 131.0; break;  // ±250°/s，转换因子为131
        case 0x08: factor = 65.5; break;   // ±500°/s，转换因子为65.5
        case 0x10: factor = 32.8; break;   // ±1000°/s，转换因子为32.8
        case 0x18: factor = 16.4; break;   // ±2000°/s，转换因子为16.4
        default: factor = 131.0; break;     // 默认量程范围为±250°/s
    }
    // 将原始陀螺仪值转换为°/s单位
    return value / factor;
}

2.5 驱动代码实现

2.5.1 初始化函数

初始化MPU6050，设置工作模式、采样率和传感器量程。

复制代码

#define Gyro_Range 0x18
#define Accel_Range 0x18

//初始化硬件MPU6050
void MPU6050_Init(void)
{
	IIC_Init();//初始化IIC
	MPU6050_WriteReg( MPU6050_PWR_MGMT_1, 0x81);//复位选用陀螺仪时钟作为时钟源
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);//使能3轴加速度和3轴陀螺仪
	
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);	//设置采样分频器为10
	
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);	//设置数字低通滤波器
	
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, Gyro_Range); //配置陀螺仪满量程+2000dps
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, Accel_Range);//配置陀螺仪满量程±16g
	
	Delay_ms(50);
}

2.5.2 数据读取函数

读取加速度和陀螺仪数据，并转换为实际的物理量。

复制代码

void MPU6050_ReadSensors(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) 
{
    int16_t raw_ax, raw_ay, raw_az;  // 用于存储原始加速度数据
    int16_t raw_gx, raw_gy, raw_gz;  // 用于存储原始陀螺仪数据

    // 读取加速度计的原始数据
    MPU6050_ReadAccel(&raw_ax, &raw_ay, &raw_az);

    // 读取陀螺仪的原始数据
    MPU6050_ReadGyro(&raw_gx, &raw_gy, &raw_gz);

    // 将加速度计的原始数据转换为浮点数形式（单位：g）
    *ax = MPU6050_ConvertAccel(raw_ax, Accel_Range); // ±2g量程
    *ay = MPU6050_ConvertAccel(raw_ay, Accel_Range); // ±2g量程
    *az = MPU6050_ConvertAccel(raw_az, Accel_Range); // ±2g量程

    // 将陀螺仪的原始数据转换为浮点数形式（单位：°/s）
    *gx = MPU6050_ConvertGyro(raw_gx, Gyro_Range); // ±250°/s量程
    *gy = MPU6050_ConvertGyro(raw_gy, Gyro_Range); // ±250°/s量程
    *gz = MPU6050_ConvertGyro(raw_gz, Gyro_Range); // ±250°/s量程
}

2.6 示例代码

以下是一个完整的示例代码，展示如何初始化MPU6050并读取加速度和陀螺仪数据：

复制代码

#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "MPU6050.h"
#include "MPU6050_Reg.h"
#include "Usart.h"

int main(void) {
   
    // 初始化MPU6050
    MPU6050_Init();

    while (1) {
        float ax, ay, az;
        float gx, gy, gz;

        // 读取传感器数据
        MPU6050_ReadSensors(&ax, &ay, &az, &gx, &gy, &gz);

        // 打印数据
        printf("Accel: X=%.2fg, Y=%.2fg, Z=%.2fg\r\n", ax, ay, az);
        printf("Gyro: X=%.2f°/s, Y=%.2f°/s, Z=%.2f°/s\r\n", gx, gy, gz);

        // 延时
        delay_ms(1000);
    }
}

三. 姿态解算

为什么用姿态解算计算设备姿态角👀因为计算设备姿态角上一般所选用的惯性传感器，都是mems器件，精度相对较差，同时单个陀螺仪、加速度计、地磁计传感器无法得到满意的姿态角信息，所以需要运用到融合算法，进行姿态估计。

姿态解算的目标是通过融合MPU6050的加速度计和陀螺仪数据，得到一个准确的姿态估计的👉即计算出设备在三维空间（坐标系）中的姿态角（俯仰角、横滚角和偏航角）。姿态角是描述物体在空间中旋转状态的三个角度，通常用于++飞控、机器人、VR设备++等领域。

3.1 姿态角与坐标系

3.1.1 姿态角的定义

姿态角描述了设备的机体坐标系相对于惯性坐标系的旋转关系（以我图中的画的坐标系作为参考）

1. 俯仰角（Pitch） ：表示设备绕X轴的旋转角度，范围为**-90°到+90°**。俯仰角描述了设备相对于水平面的上下倾斜程度。

正值：物体向上倾斜（机头向上）。
负值：物体向下倾斜（机头向下）。

2. 横滚角（Roll） ：表示设备绕Y轴的旋转角度，范围为**-180°到+180°**。横滚角描述了设备相对于水平面的左右倾斜程度。

正值：物体向右倾斜（右翼向下左翼向上）。
负值：物体向左倾斜（左翼向下右翼向上）。

3. 偏航角（Yaw） ：表示设备绕Z轴的旋转角度，范围为**-180°到+180°**。偏航角描述了设备在水平面上的旋转方向。

正值：物体顺时针旋转（向东偏航）。
负值：物体逆时针旋转（向西偏航）。

++注：++在姿态解算中，姿态角的定义通常是以惯性坐标系（世界坐标系）作为参考的。惯性坐标系是一个固定在空间中的参考系，通常以地球的重力方向为基准。

3.1.2 坐标系的概念

在姿态解算中，坐标系的选择至关重要。通常使用以下两种坐标系：

惯性坐标系（世界坐标系）：固定在空间中的坐标系，通常以地球的重力方向为Z轴，水平面为XY平面。
机体坐标系（传感器坐标系）：固定在设备上的坐标系，通常以设备的几何中心为原点，X轴指向设备的前方向，Y轴指向设备的右方向，Z轴指向设备的下方向。

姿态角描述了机体坐标系相对于惯性坐标系的旋转关系。通过姿态解算，可以将机体坐标系下的测量数据转换到惯性坐标系下，从而实现对设备姿态的描述。➡️抽象来说：姿态解算是"机体坐标系 "与"惯性坐标系"之间的转换关系！

3.1.3 在姿态解算中的应用

在姿态解算中，通过传感器（如加速度计、陀螺仪和磁力计）获取的数据，可以计算出这些欧拉角。这些角度通常用于描述飞行器、机器人或其他设备的姿态。

加速度计：用于测量重力方向，从而计算出俯仰角和横滚角。
陀螺仪：用于测量角速度，通过积分计算出姿态变化。
磁力计：用于测量地磁场方向，从而计算出偏航角。

**仅使用加速度计和陀螺仪的数据也可以求出欧拉角，但会有一定的局限性和误差。**加速度计可以提供关于重力方向的信息，从而帮助确定俯仰角和横滚角，而陀螺仪可以提供角速度信息，通过积分可以得到姿态变化。然而，由于陀螺仪的积分误差和加速度计在动态情况下的不准确性，仅使用这两个传感器的姿态解算通常不如加入磁力计的数据准确。

仅使用加速度计和陀螺仪的姿态解算方法

加速度计数据：
- 加速度计在静止状态下可以测量重力加速度，从而确定设备相对于重力方向的俯仰角和横滚角。
- 在动态情况下，加速度计的数据会受到线性加速度的影响，导致姿态估计不准确。
陀螺仪数据：
- 陀螺仪可以测量角速度，通过积分可以得到姿态变化。
- 陀螺仪的积分误差会导致姿态估计随时间漂移。

3.2姿态解算算法

在姿态解算中，姿态可以通过多种方式表示 ，常见的有++欧拉角 、四元数和++ ++旋转矩阵++。 每种表示方法都有其特点和应用场景，且每个姿态表示方法可以相互转换。

在姿态解算中，常见的算法包括：

四元数法：通过四元数融合加速度计和陀螺仪数据，计算出设备的姿态。
互补滤波法：结合加速度计和陀螺仪数据，通过互补滤波器修正姿态角。
卡尔曼滤波法：一种更高级的滤波算法，能够更精确地融合传感器数据，适用于复杂的姿态解算

关于上面的所提到的每一个姿态表示方法和姿态解算算法并没有逐个的展开讲解，如果大家感兴趣的话可以自行查阅，这里我们主要讲四元数法应用！

3.2.1 四元数姿态解算实现

四元数是一种用于表示三维旋转的数学工具，由四个分量组成：q0,q1,q2,q3。其中，q0 是实部，q1,q2,q3 是虚部。四元数可以表示为：q=q0+q1i+q2j+q3k

相比欧拉角，它具有更好的数值稳定性和抗奇异性的特点，避免欧拉角中的"万向锁"问题。以下是四元数姿态解算的基本步骤：

初始化四元数：初始状态下，四元数设为单位四元数，即 q0=1,q1=q2=q3=0。
读取传感器数据：从MPU6050读取加速度计和陀螺仪的原始数据，并将陀螺仪数据转换为弧度。
加速度计归一化：将加速度计数据归一化，以提取重力方向。
计算姿态误差：通过加速度计和四元数推导出的重力方向的差异，计算姿态误差。
互补滤波：将姿态误差反馈到四元数更新中，结合陀螺仪数据，通过互补滤波器修正四元数。
四元数更新：通过微分方程更新四元数，并进行归一化处理。
计算欧拉角：将四元数转换为欧拉角，得到俯仰角、横滚角和偏航角。

3.2.2 四元数姿态解算算法

3.2.2.1 初始化四元数

初始化四元数为单位四元数，表示初始姿态为零。

复制代码

//定义一个关于四元数的结构体变量
typedef struct 
{
    float q0, q1, q2, q3;
} Quaternion;

Quaternion q = {1.0f, 0.0f, 0.0f, 0.0f};  // 初始化四元数 表示初始姿态为水平。

3.2.2.2 读取传感器数据

从MPU6050读取加速度计和陀螺仪的原始数据，并将陀螺仪数据转换为弧度。

复制代码

void MPU6050_ReadSensors(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
{
    int16_t raw_ax, raw_ay, raw_az;
    int16_t raw_gx, raw_gy, raw_gz;

    MPU6050_ReadAccel(&raw_ax, &raw_ay, &raw_az);
    MPU6050_ReadGyro(&raw_gx, &raw_gy, &raw_gz);

    *ax = MPU6050_ConvertAccel(raw_ax, Accel_Range);  // ±2g
    *ay = MPU6050_ConvertAccel(raw_ay, Accel_Range);
    *az = MPU6050_ConvertAccel(raw_az, Accel_Range);

    *gx = MPU6050_ConvertGyro(raw_gx, Gyro_Range) * AtR;  // ±250°/s
    *gy = MPU6050_ConvertGyro(raw_gy, Gyro_Range) * AtR;
    *gz = MPU6050_ConvertGyro(raw_gz, Gyro_Range) * AtR;
}

3.2.2.3 加速度计归一化

将加速度计数据归一化，提取重力方向

复制代码

void NormalizeAccel(float *ax, float *ay, float *az) 
{
    float norm = sqrt(*ax * *ax + *ay * *ay + *az * *az);
    *ax /= norm;
    *ay /= norm;
    *az /= norm;
}

3.2.2.4 计算姿态误差

通过加速度计和四元数推导出的重力方向的差异，计算姿态误差。

复制代码

void ComputeError(float ax, float ay, float az, float *error_x, float *error_y, float *error_z) 
{
    float gravity_x = 2 * (q.q1 * q.q3 - q.q0 * q.q2);
    float gravity_y = 2 * (q.q0 * q.q1 + q.q2 * q.q3);
    float gravity_z = 1 - 2 * (q.q1 * q.q1 + q.q2 * q.q2);

    *error_x = (ay * gravity_z - az * gravity_y);
    *error_y = (az * gravity_x - ax * gravity_z);
    *error_z = (ax * gravity_y - ay * gravity_x);
}

3.2.2.5 四元数更新

结合陀螺仪数据，通过微分方程更新四元数，并进行归一化处理，避免数值误差会越积越大。

复制代码

void UpdateQuaternion(float gx, float gy, float gz, float error_x, float error_y, float error_z, float dt) 
{
    float Kp = 0.5f;  // 互补滤波系数

    float q0_dot = -q.q1 * gx - q.q2 * gy - q.q3 * gz;
    float q1_dot = q.q0 * gx - q.q3 * gy + q.q2 * gz;
    float q2_dot = q.q3 * gx + q.q0 * gy - q.q1 * gz;
    float q3_dot = -q.q2 * gx + q.q1 * gy + q.q0 * gz;

    q.q0 += (q0_dot + Kp * error_x) * dt;
    q.q1 += (q1_dot + Kp * error_y) * dt;
    q.q2 += (q2_dot + Kp * error_z) * dt;
    q.q3 += (q3_dot) * dt;

    // 归一化四元数，避免数值误差会越积越大
    float norm = sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3);
    q.q0 /= norm;
    q.q1 /= norm;
    q.q2 /= norm;
    q.q3 /= norm;
}

3.2.2.6 计算欧拉角

将四元数转换为欧拉角，得到俯仰角、横滚角和偏航角。

复制代码

void ComputeEulerAngles(float *pitch, float *roll, float *yaw) 
{
    *roll = atan2(2 * (q.q2 * q.q3 + q.q0 * q.q1), q.q0 * q.q0 - q.q1 * q.q1 - q.q2 * q.q2 + q.q3 * q.q3);
    *pitch = asin(-2 * (q.q1 * q.q3 - q.q0 * q.q2));
    *yaw = atan2(2 * (q.q1 * q.q2 + q.q0 * q.q3), q.q0 * q.q0 + q.q1 * q.q1 - q.q2 * q.q2 - q.q3 * q.q3);

    *roll *= RtA;  // 弧度转角度
    *pitch *= RtA;
    *yaw *= RtA;
}

这些公式是基于四元数到欧拉角的转换关系，具体推导可以参考相关数学文献

3.2.2.7 主函数

将上述步骤整合到主函数中，实现完整的姿态解算。

复制代码

#include "mpu6050.h"
#include "math.h"

#define PI 3.1415926535f
#define RtA 57.2957795f  // 弧度转角度
#define AtR 0.0174532925f  // 角度转弧度

typedef struct 
{
    float q0, q1, q2, q3;
} Quaternion;

Quaternion q = {1.0f, 0.0f, 0.0f, 0.0f};  // 初始化四元数

void GetAngles(float *pitch, float *roll, float *yaw, float dt) 
{
    float ax, ay, az, gx, gy, gz;
    MPU6050_ReadSensors(&ax, &ay, &az, &gx, &gy, &gz);

    NormalizeAccel(&ax, &ay, &az);

    float error_x, error_y, error_z;
    ComputeError(ax, ay, az, &error_x, &error_y, &error_z);

    UpdateQuaternion(gx, gy, gz, error_x, error_y, error_z, dt);

    ComputeEulerAngles(pitch, roll, yaw);
}

int main(void) 
{
    // 初始化MPU6050
    MPU6050_Init();

    while (1) 
    {
        float pitch, roll, yaw;
        GetAngles(&pitch, &roll, &yaw, 0.01f);  // 假设采样间隔为10ms，保证一定的实时性

        // 打印姿态角
        printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\r\n", pitch, roll, yaw);

        // 延时
        delay_ms(10);
    }
}

3.3 注意事项

小伙伴们，在动手实现姿态解算的时候，有几个要点得留心：

硬件连接要稳：MPU6050和STM32的IIC通信线路（SDA和SCL）一定要接对，不然通信可能会出问题。
传感器得校准：加速度计和陀螺仪用之前最好校准一下，不然数据可能会不准。静止状态下多读几次数据，算个平均值，这样能更准。
数据别急躁：加速度计和陀螺仪的数据可能会有点"毛躁"，用低通滤波器或者滑动平均滤波器给它"梳梳毛"，数据会更平滑。
算法参数要调 ：四元数解算里的互补滤波器增益（比如Kp）很重要，得根据你的实际需求调整，不然解算出来的姿态可能会"飘"。
代码要细心：四元数更新后一定要归一化，不然数值误差会越积越大。还有，采样频率别太低，不然实时性会受影响。
调试别慌：遇到数据跳变或者通信失败，别急，先检查硬件连接，再看看代码逻辑。慢慢来，总能找到问题所在。

总之，技术这事儿，耐心和细心是关键。希望这些小提示能帮到你，祝你顺利搞定姿态解算！

这里我只是带大家简单理解如何操作实现的，如果大家有不理解的可以来问，欢迎大家评论，后面我可能会为大家再出一篇关于姿态解算详细讲解😄

四. 应用与调试

4.1 数据可视化

串口打印：通过串口打印姿态角（俯仰、横滚、偏航），方便实时观察。
上位机软件：用LabVIEW、MATLAB等工具接收串口数据，绘制姿态变化曲线。
显示屏：用LCD或LED显示姿态角，直观展示当前姿态。

4.2 常见问题与调试技巧

数据跳变：可能是传感器干扰或算法问题。检查硬件连接，增加滤波算法。
数据不准确：可能是传感器未校准或参数设置不当。校准传感器，调整算法参数。
通信失败：可能是硬件连接或IIC配置错误。检查线路，确保通信参数正确。

4.3 应用实例

姿态测量：用于无人机、机器人等设备的姿态控制。
运动检测：实现计步器、手势识别等功能。
VR设备：跟踪头部运动，提供沉浸式体验。

五. 总结

不知不觉，我们已经走到了文章的尾声，说实话，这过程里，我也是边写边学，收获满满。通过这篇文章的介绍，你就可以掌握了如何使用STM32通过IIC接口驱动MPU6050，并实现基于四元数的姿态解算。姿态解算的结果可以通过多种方式可视化，并应用于实际项目中。在调试过程中，注意数据异常处理和精度优化，确保姿态解算的准确性和稳定性。

最后，姿态解算这事儿，还有很多可以深挖的。比如，要是再加个磁力计，精度说不定还能再上个台阶。或者，把解算结果用到无人机、机器人上，让它们动起来更稳当。这些，就留给大家去探索啦！希望这篇文章能给你带来些启发，要是觉得有用，别忘了点赞哦！下次见！