四元数求姿态角的代码c语言

以下是使用四元数计算姿态角的C语言代码示例：

#include <math.h>

typedef struct {
    double w, x, y, z;
} Quaternion;

typedef struct {
    double roll, pitch, yaw;
} EulerAngles;

EulerAngles quaternionToEuler(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (fabs(sinp) >= 1)
        angles.pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = atan2(siny_cosp, cosy_cosp);

    return angles;
}

其中，Quaternion是四元数类型，EulerAngles是欧拉角（姿态角）类型。quaternionToEuler函数用于将四元数转换为欧拉角（roll、pitch、yaw），并返回一个EulerAngles对象。