基于STM32的四轴飞行器程序

　　// 定义四元数类型。

　　typedef struct

　　{

　　float w;

　　float x;

　　float y;

　　float z;

　　}quaternion;

　　// 规范化四元数。

　　void quaternion_normalize（quaternion * q）

　　{

　　float norm_r = 1.0f / sqrtf（q-》w*q-》w + q-》x*q-》x + q-》y*q-》y + q-》z*q-》z）;

　　q-》w *= norm_r;

　　q-》x *= norm_r;

　　q-》y *= norm_r;

　　q-》z *= norm_r;

　　}

　　// 四元数相乘

　　// left ： 被乘数，输入。

　　// right ： 乘数，输入。

　　// result ： 积，输出。

　　void quaternion_mult（quaternion * result，const quaternion * left，const quaternion * right）

　　{

　　result-》w = left-》w * right-》w - left-》x * right-》x - left-》y * right-》y - left-》z * right-》z;

　　result-》x = left-》x * right-》w + left-》w * right-》x + left-》y * right-》z - left-》z * right-》y;

　　result-》y = left-》y * right-》w + left-》w * right-》y + left-》z * right-》x - left-》x * right-》z;

　　result-》z = left-》z * right-》w + left-》w * right-》z + left-》x * right-》y - left-》y * right-》x;

　　}

　　// 用四元数来旋转向量。

　　void quaternion_rotateVector（const quaternion * rotation，const float from［3］，float to［3］）

　　{