IMU 四元数 转换姿态 C代码

以下是将IMU传感器的四元数转换为姿态角（欧拉角）的C代码：

#include <math.h>

// 将四元数转换为欧拉角（姿态）
void quat_to_euler(float q0, float q1, float q2, float q3, float *roll, float *pitch, float *yaw)
{
    float sinr_cosp = 2.0f * (q0 * q1 + q2 * q3);
    float cosr_cosp = 1.0f - 2.0f * (q1 * q1 + q2 * q2);
    *roll = atan2f(sinr_cosp, cosr_cosp);

    float sinp = 2.0f * (q0 * q2 - q3 * q1);
    if (fabsf(sinp) >= 1.0f)
        *pitch = copysignf(M_PI / 2.0f, sinp);
    else
        *pitch = asinf(sinp);

    float siny_cosp = 2.0f * (q0 * q3 + q1 * q2);
    float cosy_cosp = 1.0f - 2.0f * (q2 * q2 + q3 * q3);
    *yaw = atan2f(siny_cosp, cosy_cosp);
}

输入参数：

- `q0`，`q1`，`q2`，`q3`：四元数的四个分量。

输出参数：

- `roll`，`pitch`，`yaw`：分别为绕X轴、Y轴、Z轴旋转的欧拉角。单位为弧度。

注：这段代码中的 `atan2f` 和 `asinf` 函数可以使用标准库中的 `atan2` 和 `asin` 函数替代。