Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Math: change signs in quaternion library

Browse Source 
thanks to Justin for the suggestion
This commit is contained in:
Andrew Tridgell 2012-03-12 22:50:08 +11:00
parent af9ce0cfc9
commit d948a28e7c
1 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Math/quaternion.cpp
View File

@ -34,12 +34,12 @@ void Quaternion::rotation_matrix(Matrix3f &m)
m.a.x = 1-2*(q3q3 + q4q4); m.a.x = 1-2*(q3q3 + q4q4);
m.a.y = 2*(q2q3 - q1q4); m.a.y = 2*(q2q3 - q1q4);
m.a.z = - 2*(q2q4 + q1q3); m.a.z = 2*(q2q4 + q1q3);
m.b.x = 2*(q2q3 + q1q4); m.b.x = 2*(q2q3 + q1q4);
m.b.y = 1-2*(q2q2 + q4q4); m.b.y = 1-2*(q2q2 + q4q4);
m.b.z = -2*(q3q4 - q1q2); m.b.z = 2*(q3q4 - q1q2);
m.c.x = -2*(q2q4 - q1q3); m.c.x = 2*(q2q4 - q1q3);
m.c.y = -2*(q3q4 + q1q2); m.c.y = 2*(q3q4 + q1q2);
m.c.z = 1-2*(q2q2 + q3q3); m.c.z = 1-2*(q2q2 + q3q3);
} }
@ -61,10 +61,8 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
float cr2 = cos(roll*0.5); float cr2 = cos(roll*0.5);
float cp2 = cos(pitch*0.5); float cp2 = cos(pitch*0.5);
float cy2 = cos(yaw*0.5); float cy2 = cos(yaw*0.5);
// the sign reversal here is due to the different conventions float sr2 = sin(roll*0.5);
// in the madgwick quaternion code and the rest of APM float sp2 = sin(pitch*0.5);
float sr2 = -sin(roll*0.5);
float sp2 = -sin(pitch*0.5);
float sy2 = sin(yaw*0.5); float sy2 = sin(yaw*0.5);
q1 = cr2*cp2*cy2 + sr2*sp2*sy2; q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
@ -77,12 +75,12 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
void Quaternion::to_euler(float *roll, float *pitch, float *yaw) void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
{ {
if (roll) { if (roll) {
*roll = -(atan2(2.0*(q1*q2 + q3*q4), *roll = (atan2(2.0*(q1*q2 + q3*q4),
1 - 2.0*(q2*q2 + q3*q3))); 1 - 2.0*(q2*q2 + q3*q3)));
} }
if (pitch) { if (pitch) {
// we let safe_asin() handle the singularities near 90/-90 in pitch // we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = -safe_asin(2.0*(q1*q3 - q4*q2)); *pitch = safe_asin(2.0*(q1*q3 - q4*q2));
} }
if (yaw) { if (yaw) {
*yaw = atan2(2.0*(q1*q4 + q2*q3), *yaw = atan2(2.0*(q1*q4 + q2*q3),