aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: allow null pointers in Quaternion::to_euler()

Browse Source 
this matches the Matrix3f method
This commit is contained in:
Andrew Tridgell 2012-03-10 17:05:54 +11:00
parent 16c95236c0
commit 5c6368bad3
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Math/quaternion.cpp
View File

@ -76,10 +76,16 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
// create eulers from a quaternion
void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
{
*roll = -(atan2(2.0*(q1*q2 + q3*q4),
1 - 2.0*(q2*q2 + q3*q3)));
// we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = -safe_asin(2.0*(q1*q3 - q4*q2));
*yaw = atan2(2.0*(q1*q4 + q2*q3),
1 - 2.0*(q3*q3 + q4*q4));
if (roll) {
*roll = -(atan2(2.0*(q1*q2 + q3*q4),
1 - 2.0*(q2*q2 + q3*q3)));
}
if (pitch) {
// we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = -safe_asin(2.0*(q1*q3 - q4*q2));
}
if (yaw) {
*yaw = atan2(2.0*(q1*q4 + q2*q3),
1 - 2.0*(q3*q3 + q4*q4));
}
}