From a8fd31a5e1d9bfb7cb7b586ad5339a44737cb041 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Mar 2012 22:41:10 +1100 Subject: [PATCH] AP_Math: added quaternion->matrix and earth frame routines this will be used for 3d acceleration --- libraries/AP_Math/AP_Math.cpp | 53 +++++++++++++++++++++++++++++------ libraries/AP_Math/AP_Math.h | 11 ++++++-- 2 files changed, 54 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index e0af504ea3..28a49d7aa6 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -56,7 +56,7 @@ void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw) // calculate euler angles from a rotation matrix // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf -void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw) +void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw) { if (pitch != NULL) { *pitch = -safe_asin(m.c.x); @@ -73,14 +73,14 @@ void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw) // create a quaternion from Euler angles void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw) { - float cr2 = cos(roll/2); - float cp2 = cos(pitch/2); - float cy2 = cos(yaw/2); + float cr2 = cos(roll*0.5); + float cp2 = cos(pitch*0.5); + float cy2 = cos(yaw*0.5); // the sign reversal here is due to the different conventions // in the madgwick quaternion code and the rest of APM - float sr2 = -sin(roll/2); - float sp2 = -sin(pitch/2); - float sy2 = sin(yaw/2); + float sr2 = -sin(roll*0.5); + float sp2 = -sin(pitch*0.5); + float sy2 = sin(yaw*0.5); q.q1 = cr2*cp2*cy2 + sr2*sp2*sy2; q.q2 = sr2*cp2*cy2 - cr2*sp2*sy2; @@ -89,7 +89,7 @@ void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw) } // create eulers from a quaternion -void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw) +void euler_from_quaternion(const Quaternion &q, float *roll, float *pitch, float *yaw) { *roll = -(atan2(2.0*(q.q1*q.q2 + q.q3*q.q4), 1 - 2.0*(q.q2*q.q2 + q.q3*q.q3))); @@ -98,3 +98,40 @@ void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw) *yaw = atan2(2.0*(q.q1*q.q4 + q.q2*q.q3), 1 - 2.0*(q.q3*q.q3 + q.q4*q.q4)); } + +// convert a quaternion to a rotation matrix +void quaternion_to_rotation_matrix(const Quaternion &q, Matrix3f &m) +{ + float q3q3 = q.q3 * q.q3; + float q3q4 = q.q3 * q.q4; + float q2q2 = q.q2 * q.q2; + float q2q3 = q.q2 * q.q3; + float q2q4 = q.q2 * q.q4; + float q1q2 = q.q1 * q.q2; + float q1q3 = q.q1 * q.q3; + float q1q4 = q.q1 * q.q4; + float q4q4 = q.q4 * q.q4; + + m.a.x = 1-2*(q3q3 + q4q4); + m.a.y = 2*(q2q3 - q1q4); + m.a.z = - 2*(q2q4 + q1q3); + m.b.x = 2*(q2q3 + q1q4); + m.b.y = 1-2*(q2q2 + q4q4); + m.b.z = -2*(q3q4 - q1q2); + m.c.x = -2*(q2q4 - q1q3); + m.c.y = -2*(q3q4 + q1q2); + m.c.z = 1-2*(q2q2 + q3q3); +} + +// convert a vector in earth frame to a vector in body frame, +// assuming body current rotation is given by a quaternion +void quaternion_earth_to_body(const Quaternion &q, Vector3f &v) +{ + Matrix3f m; + // we reverse z before and afterwards because of the differing + // quaternion conventions from APM conventions. + v.z = -v.z; + quaternion_to_rotation_matrix(q, m); + v = m * v; + v.z = -v.z; +} diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 6975ccc28e..c2a1b89af3 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -24,10 +24,17 @@ float safe_sqrt(float v); void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw); // calculate euler angles from a rotation matrix -void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw); +void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw); // create a quaternion from Euler angles void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw); // create eulers from a quaternion -void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw); +void euler_from_quaternion(const Quaternion &q, float *roll, float *pitch, float *yaw); + +// convert a quaternion to a rotation matrix +void quaternion_to_rotation_matrix(const Quaternion &q, Matrix3f &m); + +// convert a vector in earth frame to a vector in body frame, +// assuming body current rotation is given by a quaternion +void quaternion_earth_to_body(const Quaternion &q, Vector3f &v);