Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues 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 // create eulers from a quaternion
void Quaternion::to_euler(float *roll, float *pitch, float *yaw) void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
{ {
*roll = -(atan2(2.0*(q1*q2 + q3*q4), if (roll) {
1 - 2.0*(q2*q2 + q3*q3))); *roll = -(atan2(2.0*(q1*q2 + q3*q4),
// we let safe_asin() handle the singularities near 90/-90 in pitch 1 - 2.0*(q2*q2 + q3*q3)));
*pitch = -safe_asin(2.0*(q1*q3 - q4*q2)); }
*yaw = atan2(2.0*(q1*q4 + q2*q3), if (pitch) {
1 - 2.0*(q3*q3 + q4*q4)); // 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));
}
} }