陀螺仪MPU6050、MPU6500姿态解算。 软件滤波，四元素。 工程是GD32F303芯片的。 c语言文件，可以移植stm32等任意平台。 带freertos系统。 输出pitch，roll，yaw

最近在GD32F303上折腾MPU6050姿态解算，踩了不少坑总算把三轴姿态角给整出来了。这玩意儿说起来简单，真要自己动手从零搞起，没点耐心还真不行。先说硬件配置，MPU6050和MPU6500这俩兄弟用起来差不多，都是I2C接口，注意电源要稳，最好单独供电别跟数字电路混用。

先上个初始化代码片段：

void mpu_init(void)
{
    i2c_write_byte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x80); //复位
    vTaskDelay(50);
    i2c_write_byte(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x07);
    i2c_write_byte(MPU6050_ADDR, MPU6050_CONFIG, 0x06);
    i2c_write_byte(MPU6050_ADDR, MPU6050_GYRO_CONFIG, 0x18); //±2000dps
    i2c_write_byte(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 0x10); //±8g
}

这里有个细节要注意，GD32的I2C时序和STM32略有不同，如果发现通信失败，记得调整时钟频率配置。我当初就卡在这儿半天，最后发现是GD32的I2C时钟寄存器设置需要多右移一位。

姿态解算核心算法用四元数，配合互补滤波食用更佳。先上关键的结构体定义：

typedef struct {
    float q0, q1, q2, q3;
    float integralFBx, integralFBy, integralFBz;
} Attitude_Quaternion;

互补滤波实现部分，这里用加速度计修正陀螺仪漂移：

void complementary_filter(IMU_Data *imu, Attitude_Quaternion *q, float dt)
{
    // 加速度计归一化
    float acc_norm = 1.0f / sqrtf(imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az);
    imu->ax *= acc_norm;
    imu->ay *= acc_norm;
    imu->az *= acc_norm;

    // 计算误差
    float vx = 2*(q->q1*q->q3 - q->q0*q->q2);
    float vy = 2*(q->q0*q->q1 + q->q2*q->q3);
    float ez = vx*imu->ay - vy*imu->ax;

    // 积分反馈
    q->integralFBz += Ki * ez * dt;
    imu->gz += q->integralFBz + Kp * ez;
}

这个滤波参数Kp和Ki需要实测调整，建议从0.5开始慢慢调。有个偷懒的方法——接上调试器，看着波形调参最直观。

陀螺仪MPU6050、MPU6500姿态解算。 软件滤波，四元素。 工程是GD32F303芯片的。 c语言文件，可以移植stm32等任意平台。 带freertos系统。 输出pitch，roll，yaw

FreeRTOS任务拆分有讲究，建议分两个任务：一个高频任务专门读传感器数据（500Hz以上），另一个中频任务做解算（200Hz左右）。实测这样既能保证数据实时性，又不会让CPU负载太高。

最后转换欧拉角的部分要特别注意奇异点问题：

void quat_to_euler(Attitude_Quaternion *q, Euler_Angle *angle)
{
    // roll
    float sinr_cosp = 2*(q->q0*q->q1 + q->q2*q->q3);
    float cosr_cosp = 1 - 2*(q->q1*q->q1 + q->q2*q->q2);
    angle->roll = atan2f(sinr_cosp, cosr_cosp);

    // pitch
    float sinp = 2*(q->q0*q->q2 - q->q3*q->q1);
    if(fabsf(sinp) >= 1)
        angle->pitch = copysignf(M_PI/2, sinp); // 处理90度奇异点
    else
        angle->pitch = asinf(sinp);

    // yaw
    float siny_cosp = 2*(q->q0*q->q3 + q->q1*q->q2);
    float cosy_cosp = 1 - 2*(q->q2*q->q2 + q->q3*q->q3);
    angle->yaw = atan2f(siny_cosp, cosy_cosp);
}

实测发现当pitch接近±90度时，roll和yaw会开始漂，这是欧拉角的固有问题。如果项目对全姿态有要求，建议直接用四元数做后续处理。

移植到其他平台时，重点关注三个地方：I2C/SPI驱动、定时器时钟源、浮点运算支持。GD32F303有硬件浮点，爽得很。要是换到没有FPU的芯片，记得把浮点运算改成定点数或者查表法。

最后吐槽下MPU6050的温漂，静止状态下yaw角每小时能飘个5-8度。解决办法是上磁力计做九轴融合，或者简单粗暴——每隔几分钟用加速度计重新校准零点。