The conversion between Euler angle and four-yuan number in flight control attitude calculation

Euler angle turn four yuan

void Cjx_from_euler (float roll, float pitch, float yaw)

{

float Cr2 = COSF (roll*0.5f);

float CP2 = COSF (pitch*0.5f);

float cy2 = COSF (yaw*0.5f);

Float SR2 = Sinf (roll*0.5f);

Float SP2 = Sinf (pitch*0.5f);

float sy2 = Sinf (yaw*0.5f);

Q1 = cr2*cp2*cy2 + sr2*sp2*sy2;

q2 = Sr2*cp2*cy2-cr2*sp2*sy2;

Q3 = Cr2*sp2*cy2 + sr2*cp2*sy2;

Q4 = CR2*CP2*SY2-SR2*SP2*CY2;

}

#define RAD_TO_DEG 57.29577951f

Radian to Angle

Float degrees (float rad)

{

Return rad * RAD_TO_DEG;

}

Four Yuan to Euler angle

void Cjx_to_euler (float *roll, float *pitch, float *yaw)

{

if (roll) {

*roll = Degrees (atan2f (2.0f* (q1*q2 + q3*q4), 1-2.0f* (Q2*Q2 + q3*q3));

}

if (pitch) {

Use Safe_asin () to handle singularities when pitch approaches 90/-90

*pitch = Degrees (Safe_asin (2.0f* (Q1*Q3-Q2*Q4)));

}

if (yaw) {

*yaw = Degrees (atan2f (2.0f* (q2*q3-q1*q4), 2.0f* (q1*q1 + q2*q2)-1));

}

}

This article is from the "C Little Wolf" blog, please make sure to keep this source http://caijinxun.blog.51cto.com/9101387/1872102

The conversion between Euler angle and four-yuan number in flight control attitude calculation