**Applicable models: **M600 series, A3, N3, M210 series, M210 V2 series

**SDK version: **OSDK3.7 & OSDK3.8

Telemetry provides a quaternion topic (TOPIC_QUATERNION) but does not provide a way to obtain the attitude angle. However, the quaternion can be converted into an attitude angle by means of Euler's formula. The conversion procedure is as follows:

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); //Row

ans.y = atan2(t3, t4); //pitch

ans.z = atan2(t1, t0); //Yaw

return ans;

}

## Comments

0 comments

Please sign in to leave a comment.