用c语言写一个姿态解算的代码

好的，以下是一个简单的姿态解算的代码示例：

#include <stdio.h>
#include <math.h>

void quaternion_to_euler(float q0, float q1, float q2, float q3, float *roll, float *pitch, float *yaw) {
    float sinr_cosp = 2 * (q0 * q1 + q2 * q3);
    float cosr_cosp = 1 - 2 * (q1 * q1 + q2 * q2);
    *roll = atan2(sinr_cosp, cosr_cosp);

    float sinp = 2 * (q0 * q2 - q3 * q1);
    if (fabs(sinp) >= 1)
        *pitch = copysign(M_PI / 2, sinp);
    else
        *pitch = asin(sinp);

    float siny_cosp = 2 * (q0 * q3 + q1 * q2);
    float cosy_cosp = 1 - 2 * (q2 * q2 + q3 * q3);
    *yaw = atan2(siny_cosp, cosy_cosp);
}

int main() {
    float q0 = 0.5, q1 = 0.5, q2 = 0.5, q3 = 0.5;
    float roll, pitch, yaw;
    quaternion_to_euler(q0, q1, q2, q3, &roll, &pitch, &yaw);
    printf("Roll: %f, Pitch: %f, Yaw: %f\n", roll, pitch, yaw);
    return 0;
}

这个代码使用四元数来表示姿态，并将其转换为欧拉角（roll、pitch、yaw）。