[DJI OSDK] 드론의 헤딩 각도 계산 방법

수학집어 치운지 어언 20년이 넘었는데, 필자가 드론 관련 개발을 하면서 

 

드론에서 받을 수 있는 RAW값을 이용하여 드론이나 짐벌의 헤딩을 처리해야되는 상황이 왔다. 

 

드론에서 4개의 방위(North,South,East,West)에 대한 값을 통해 발생되는 데이터를 확인하고 

 

공식을 찾다보니... 별로 없다. 아니 필자 생각에는 필자가 잘 몰라서 그런거라 생각된다. 

 

어쨋거나, 시간을 줄이기 위해 DJI 포럼에서 문의했고 발송된 결과는 아래와 같다.

 

물론, 필자는 이 메일을 보기전에 노가다 코딩으로 문제를 이미 해결한 상태라...

 

혹시 나중에 사용할 수 있을 것 같아 커뮤니티의 내용을 일부 덜어왔다. 

 

잘하면 담에 쓸 일이 생길거 같다... ㅠㅜ 수학시러..

 

-----------------------------------------------------------------------------------------------------------

 

How to convert a quaternion into a drone attitude angle | from  kyle.cai | 1 year ago Updated

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; 
}

 

이상이다.