From 9f0cb78f08ae1c93cd753583531901396ac6fb28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Mar 2012 16:44:22 +1100 Subject: [PATCH] AP_Math: re-work quaternion functions to be more C++ like thanks to Adam for the suggestion! --- libraries/AP_Math/AP_Math.cpp | 66 ------------------------- libraries/AP_Math/AP_Math.h | 13 ----- libraries/AP_Math/quaternion.cpp | 85 ++++++++++++++++++++++++++++++++ libraries/AP_Math/quaternion.h | 11 +++++ 4 files changed, 96 insertions(+), 79 deletions(-) create mode 100644 libraries/AP_Math/quaternion.cpp diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 28a49d7aa6..e97763a2b5 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -69,69 +69,3 @@ void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float } } - -// create a quaternion from Euler angles -void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw) -{ - 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*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; - q.q3 = cr2*sp2*cy2 + sr2*cp2*sy2; - q.q4 = cr2*cp2*sy2 - sr2*sp2*cy2; -} - -// create eulers from a quaternion -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))); - // we let safe_asin() handle the singularities near 90/-90 in pitch - *pitch = -safe_asin(2.0*(q.q1*q.q3 - q.q4*q.q2)); - *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 22de694877..035142f355 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -30,17 +30,4 @@ void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw) // calculate euler angles from a rotation matrix 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(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); - #endif diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp new file mode 100644 index 0000000000..f587bf1d31 --- /dev/null +++ b/libraries/AP_Math/quaternion.cpp @@ -0,0 +1,85 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * quaternion.cpp + * Copyright (C) Andrew Tridgell 2012 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "AP_Math.h" + +// return the rotation matrix equivalent for this quaternion +void Quaternion::rotation_matrix(Matrix3f &m) +{ + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q4q4 = q4 * 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 from earth to body frame +void Quaternion::earth_to_body(Vector3f &v) +{ + Matrix3f m; + // we reverse z before and afterwards because of the differing + // quaternion conventions from APM conventions. + v.z = -v.z; + rotation_matrix(m); + v = m * v; + v.z = -v.z; +} + +// create a quaternion from Euler angles +void Quaternion::from_euler(float roll, float pitch, float yaw) +{ + 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*0.5); + float sp2 = -sin(pitch*0.5); + float sy2 = sin(yaw*0.5); + + 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; +} + +// 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)); +} diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index d5b5913e82..f1932dbc29 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -33,5 +33,16 @@ public: bool is_nan(void) { return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); } + // return the rotation matrix equivalent for this quaternion + void rotation_matrix(Matrix3f &m); + + // convert a vector from earth to body frame + void earth_to_body(Vector3f &v); + + // create a quaternion from Euler angles + void from_euler(float roll, float pitch, float yaw); + + // create eulers from a quaternion + void to_euler(float *roll, float *pitch, float *yaw); }; #endif // QUATERNION_H