关键词:OSDK姿态控制,四元数欧拉角转换
姿态控制过程中,OSDK可以实时获取四元数并计算飞机姿态角,应用端可以根据飞机的姿态角来实时调整飞机角度从而控制飞机掉头。
四元数与欧拉角的换算可以参考sample中提供的代码:
Telemetry::Vector3f
toEulerAngle(void* quaternionData)
{
Telemetry::Vector3f ans;
Telemetry::Quaternion* quaternion = (Telemetry::Quaternion*)quaternionData;
double q2sqr = quaternion->q2 * quaternion->q2;
double t0 = -2.0 * (q2sqr + quaternion->q3 * quaternion->q3) + 1.0;
double t1 =
+2.0 * (quaternion->q1 * quaternion->q2 + quaternion->q0 * quaternion->q3);
double t2 =
-2.0 * (quaternion->q1 * quaternion->q3 - quaternion->q0 * quaternion->q2);
double t3 =
+2.0 * (quaternion->q2 * quaternion->q3 + quaternion->q0 * quaternion->q1);
double t4 = -2.0 * (quaternion->q1 * quaternion->q1 + q2sqr) + 1.0;
t2 = (t2 > 1.0) ? 1.0 : t2;
t2 = (t2 < -1.0) ? -1.0 : t2;
ans.x = asin(t2);
ans.y = atan2(t3, t4);
ans.z = atan2(t1, t0);
return ans;
}
评论
0 条评论
请登录写评论。