roll角 四元数转换 stm32示例

以下是一个简单的示例代码，用于将四元数转换为欧拉角，并在STM32上进行演示：

#include "stm32f4xx.h"
#include <math.h>

#define PI 3.14159265358979323846

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

void Quaternion_ToEuler(void)
{
    //计算欧拉角
    roll = atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1*q1 + q2*q2)) * 180 / PI;
    pitch = asin(2 * (q0*q2 - q1*q3)) * 180 / PI;
    yaw = atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2*q2 + q3*q3)) * 180 / PI;
}

int main(void)
{
    //初始化代码
    //...

    while (1)
    {
        //读取四元数
        q0 = ...;
        q1 = ...;
        q2 = ...;
        q3 = ...;

        //将四元数转换为欧拉角
        Quaternion_ToEuler();

        //输出欧拉角
        printf("roll: %.2f, pitch: %.2f, yaw: %.2f\n", roll, pitch, yaw);

        //延时一段时间
        HAL_Delay(100);
    }
}

请注意，此示例代码仅供参考，并且需要根据您的具体应用场景进行适当修改。