欧拉角转四元数再进行框架耦合再转回欧拉角
首先定义结构体
// 三轴欧拉角
typedef struct
{
double Roll; // roll_z
double Pitch; // pitch_x
double Yaw; // yaw_y
}GyroEulerAngle;
// 四元数结构体
typedef struct
{
float x, y, z, w;
}Quaternions;
欧拉角转四元数
//欧拉角转四元素 euler_angle:输入欧拉角 q1:输出四元素
int Conversion_Euler_to_Quaternion(Quaternions *q1, GyroEulerAngle *euler_angle)
{
double g = euler_angle->Roll * PI / 180;
double a = euler_angle->Pitch * PI / 180;
double b = euler_angle->Yaw * PI / 180;
double ca = cos(a / 2);
double sa = sin(a / 2);
double cb = cos(b / 2);
double sb = sin(b / 2);
double cg = cos(g