How to control the aircraft to turn around during OSDK attitude control?

Keywords: OSDK attitude control, quaternion Euler angle conversion

During the attitude control, OSDK can obtain the quaternion in real time and calculate the attitude angle of the UAV. And the OSDK application was able to control the drone to turn around according to the drone attitude.

The conversion of quaternion and Euler angle, please refer to the code provided in the 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;
}

Note: This article is from Chinese and is translated by machine. If there is any suggestions, please point it out and we will correct it in time