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

目录

前言

一.MPU6050模块介绍

 1.1MPU6050简介

1.2 MPU6050的引脚定义

1.3MPU6050寄存器解析

二.MPU6050驱动开发

2.1 配置寄存器

2.2对MPU6050寄存器进行读写

2.2.1 写入寄存器

2.2.2读取寄存器

2.3 初始化MPU6050

2.3.1 设置工作模式

2.3.2 配置采样率

2.3.3 启用传感器

2.4MPU6050数据读取

2.4.1 读取加速度数据

2.4.2 读取陀螺仪数据

2.4.3 数据格式转换

2.5 驱动代码实现

2.5.1 初始化函数

2.5.2 数据读取函数

3.5.2 数据读取函数

2.6 示例代码

三. 姿态解算

3.1姿态角与坐标系

3.1.1 姿态角的定义

3.1.2 坐标系的概念

3.2姿态解算算法

3.2.1四元数姿态解算实现

3.2.2四元数姿态解算算法

3.2.2.1初始化四元数

3.2.2.2读取传感器数据

3.2.2.3 加速度计归一化

3.2.2.4计算姿态误差

3.2.2.5四元数更新

3.2.2.6计算欧拉角

3.2.2.7主函数

 3.3注意事项

四. 应用与调试

4.1 数据可视化

4.2 常见问题与调试技巧

4.3 应用实例

五. 总结

前言

小伙伴们，大家好！已经迫不及待想分享新的技术了！今天，我们来搞点好玩的——MPU6050。这可是个好东西，一个集成了加速度计和陀螺仪的六轴传感器，能帮你轻松搞定姿态测量和运动检测。话不多说，直接开搞！！！干货满满，内容比较多，大家可以存下来慢慢看👀

在嵌入式系统和物联网的世界里，传感器就像是我们的“眼睛”和“耳朵”，能帮我们感知周围的一切。而MPU6050，就是这样一个超级厉害的传感器。它不仅能测加速度，还能测角速度，简直是姿态测量的神器！今天，我们就用STM32来驱动它，顺便搞搞姿态解算，让你的项目瞬间高大上起来。

一.MPU6050模块介绍

 1.1MPU6050简介

MPU6050是一款高性能的六轴运动传感器，集成了三轴加速度计和三轴陀螺仪。它能够测量加速度和角速度，广泛应用于姿态测量、运动检测、机器人控制等领域。MPU6050的主要特性如下：

• 三轴加速度计：测量范围为±2g/±4g/±8g/±16g（可选 单位g为重力加速度）。

• 三轴陀螺仪：测量范围为±250°/s/±500°/s/±1000°/s/±2000°/s（可选）。

• 数字运动处理器（DMP）：支持复杂的运动处理算法，如姿态解算。

• IIC接口：支持标准IIC通信协议，易于与微控制器连接。

• 低功耗：适合电池供电的便携式设备。

• 高精度：能够提供高精度的加速度和角速度数据。

1.2 MPU6050的引脚定义

MPU6050模块通常具有以下引脚：

• VCC：电源正极（3.3V或5V）。

• GND：电源地。

• SDA：IIC数据线。

• SCL：IIC时钟线。

• INT：中断输出引脚（可选）。

• AD0：地址选择引脚，用于设置IIC设备地址。

1.3MPU6050寄存器解析

MPU6050通过IIC接口与微控制器通信，数据存储在内部寄存器中。下面将介绍我们完成功能所主要用寄存器：

• PWR_MGMT_1：电源管理寄存器，用于设置传感器的工作模式。

• SMPLRT_DIV：采样率分频寄存器，用于设置数据采样率。

• CONFIG：配置寄存器，用于设置数字低通滤波器（DLPF）。

• GYRO_CONFIG：陀螺仪配置寄存器，用于设置测量范围。

• ACCEL_CONFIG：加速度计配置寄存器，用于设置测量范围。

• ACCEL_XOUT_H/L、ACCEL_YOUT_H/L、ACCEL_ZOUT_H/L：加速度数据寄存器。

• GYRO_XOUT_H/L、GYRO_YOUT_H/L、GYRO_ZOUT_H/L：陀螺仪数据寄存器。

通过配置这些寄存器，可以实现对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 0x30
#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 = 1365.3; break;  // ±16g，转换因子为1365.3
        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，设置工作模式、采样率和传感器量程。

void MPU6050_Init(void) 
{
    // 复位MPU6050
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);

    // 设置陀螺计量程为±250°/s
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x00);

    // 设置加速度计量程为±2g
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);

    // 设置采样率分频值为0
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x00);
}

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, 0x00); // ±2g量程
    *ay = MPU6050_ConvertAccel(raw_ay, 0x00); // ±2g量程
    *az = MPU6050_ConvertAccel(raw_az, 0x00); // ±2g量程

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

2.6 示例代码

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

#include "mpu6050.h"
#include "iic.h"

int main(void) {
    // 初始化IIC接口
    IIC_Init();

    // 初始化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 姿态角的定义

姿态角描述了设备的机体坐标系相对于惯性坐标系的旋转关系。

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

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

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

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

3.1.2 坐标系的概念

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

• 惯性坐标系（世界坐标系）：固定在空间中的坐标系，通常以地球的重力方向为Z轴，水平面为XY平面。

• 机体坐标系（传感器坐标系）：固定在设备上的坐标系，通常以设备的几何中心为原点，X轴指向设备的前方向，Y轴指向设备的右方向，Z轴指向设备的下方向。

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

3.2姿态解算算法

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

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

1. 四元数法：通过四元数融合加速度计和陀螺仪数据，计算出设备的姿态。

2. 互补滤波法：结合加速度计和陀螺仪数据，通过互补滤波器修正姿态角。

3. 卡尔曼滤波法：一种更高级的滤波算法，能够更精确地融合传感器数据，适用于复杂的姿态解算

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

3.2.1四元数姿态解算实现

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

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

1. 初始化四元数：初始状态下，四元数设为单位四元数，即 q0​=1,q1​=q2​=q3​=0。

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

3. 加速度计归一化：将加速度计数据归一化，以提取重力方向。

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

5. 互补滤波：将姿态误差反馈到四元数更新中，结合陀螺仪数据，通过互补滤波器修正四元数。

6. 四元数更新：通过微分方程更新四元数，并进行归一化处理。

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

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, 0x00);  // ±2g
    *ay = MPU6050_ConvertAccel(raw_ay, 0x00);
    *az = MPU6050_ConvertAccel(raw_az, 0x00);

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

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注意事项

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

1. 硬件连接要稳：MPU6050和STM32的IIC通信线路（SDA和SCL）一定要接对，不然通信可能会出问题。

2. 传感器得校准：加速度计和陀螺仪用之前最好校准一下，不然数据可能会不准。静止状态下多读几次数据，算个平均值，这样能更准。

3. 数据别急躁：加速度计和陀螺仪的数据可能会有点“毛躁”，用低通滤波器或者滑动平均滤波器给它“梳梳毛”，数据会更平滑。

4. 算法参数要调：四元数解算里的互补滤波器增益（比如Kp）很重要，得根据你的实际需求调整，不然解算出来的姿态可能会“飘”。

5. 代码要细心：四元数更新后一定要归一化，不然数值误差会越积越大。还有，采样频率别太低，不然实时性会受影响。

6. 调试别慌：遇到数据跳变或者通信失败，别急，先检查硬件连接，再看看代码逻辑。慢慢来，总能找到问题所在。

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

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

四. 应用与调试

4.1 数据可视化

• 串口打印：通过串口打印姿态角（俯仰、横滚、偏航），方便实时观察。

• 上位机软件：用LabVIEW、MATLAB等工具接收串口数据，绘制姿态变化曲线。

• 显示屏：用LCD或LED显示姿态角，直观展示当前姿态。

4.2 常见问题与调试技巧

• 数据跳变：可能是传感器干扰或算法问题。检查硬件连接，增加滤波算法。

• 数据不准确：可能是传感器未校准或参数设置不当。校准传感器，调整算法参数。

• 通信失败：可能是硬件连接或IIC配置错误。检查线路，确保通信参数正确。

4.3 应用实例

• 姿态测量：用于无人机、机器人等设备的姿态控制。

• 运动检测：实现计步器、手势识别等功能。

• VR设备：跟踪头部运动，提供沉浸式体验。

五. 总结

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

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