stm32 mpu6050四元数转欧拉角示例

下面是一个基于STM32和MPU6050的四元数转欧拉角的示例代码：

#include "stm32f10x.h"
#include "mpu6050.h"
#include "quaternion.h"
#include "math.h"

#define PI 3.14159265358979323846

float q0, q1, q2, q3; // 四元数
float yaw, pitch, roll; // 欧拉角

void MPU6050_Init(void);

int main(void)
{
    MPU6050_Init(); // 初始化MPU6050
    Quaternion_Init(); // 初始化四元数

    while (1)
    {
        MPU6050_ReadGyro(); // 读取陀螺仪数据
        Quaternion_Update(MPU6050_Gyro[0], MPU6050_Gyro[1], MPU6050_Gyro[2], 0.01); // 更新四元数
        Quaternion_ToEuler(&q0, &q1, &q2, &q3, &yaw, &pitch, &roll); // 四元数转欧拉角
        yaw *= 180.0 / PI; // 转换为角度制
        pitch *= 180.0 / PI;
        roll *= 180.0 / PI;
    }
}

void MPU6050_Init(void)
{
    MPU6050_InitTypeDef MPU6050_InitStruct;
    MPU6050_InitStruct.ClockSource = MPU6050_CLOCK_PLL_XGYRO; // 时钟源选择为X轴陀螺仪
    MPU6050_InitStruct.GyroXRange = MPU6050_GYRO_FS_250; // 陀螺仪量程为±250dps
    MPU6050_InitStruct.GyroYRange = MPU6050_GYRO_FS_250;
    MPU6050_InitStruct.GyroZRange = MPU6050_GYRO_FS_250;
    MPU6050_Init(&MPU6050_InitStruct);
}

其中，`quaternion.h`头文件中包含了四元数相关的函数，如`Quaternion_Init()`用于初始化四元数，`Quaternion_Update()`用于更新四元数，`Quaternion_ToEuler()`用于将四元数转换为欧拉角。

在`main()`函数中，我们首先初始化了MPU6050和四元数，然后进入一个死循环，不断读取陀螺仪数据并更新四元数。接着调用`Quaternion_ToEuler()`函数将四元数转换为欧拉角，并将结果乘以180/π转换为角度制。最终得到的`yaw`、`pitch`和`roll`即为所求的欧拉角。

需要注意的是，欧拉角的计算中可能会涉及到一些特殊情况，如万向锁等，需要特别处理。此外，欧拉角的计算还可能存在一些精度问题，需要根据实际情况做出相应的调整。