From f2e67145984911617062ffb6ed3a9efd09f1df52 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Mar 2012 14:31:47 +1100 Subject: [PATCH] AP_Math: added quaternion helper functions and a test suite --- libraries/AP_Math/AP_Math.cpp | 30 ++++ libraries/AP_Math/AP_Math.h | 7 + libraries/AP_Math/examples/eulers/Makefile | 1 + libraries/AP_Math/examples/eulers/eulers.pde | 159 +++++++++++++++++++ libraries/AP_Math/quaternion.h | 36 +++++ 5 files changed, 233 insertions(+) create mode 100644 libraries/AP_Math/examples/eulers/Makefile create mode 100644 libraries/AP_Math/examples/eulers/eulers.pde create mode 100644 libraries/AP_Math/quaternion.h diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 0541246e53..f4896ef38b 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -67,3 +67,33 @@ void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw) *yaw = atan2(m.b.x, m.a.x); } } + + +// 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); + // 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); + + 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(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)); +} diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 37cc5bac14..6975ccc28e 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -7,6 +7,7 @@ #include "vector2.h" #include "vector3.h" #include "matrix3.h" +#include "quaternion.h" #include "polygon.h" // define AP_Param types AP_Vector3f and Ap_Matrix3f @@ -24,3 +25,9 @@ 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); + +// 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); diff --git a/libraries/AP_Math/examples/eulers/Makefile b/libraries/AP_Math/examples/eulers/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/AP_Math/examples/eulers/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde new file mode 100644 index 0000000000..2f27d98b9e --- /dev/null +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -0,0 +1,159 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// Unit tests for the AP_Math euler code +// + +#include +#include +#include + +FastSerialPort(Serial, 0); + +static float rad_diff(float rad1, float rad2) +{ + float diff = rad1 - rad2; + if (diff > PI) { + diff -= 2*PI; + } + if (diff < -PI) { + diff += 2*PI; + } + return fabs(diff); +} + +static void test_euler(float roll, float pitch, float yaw) +{ + Matrix3f m; + float roll2, pitch2, yaw2; + + rotation_matrix_from_euler(m, roll, pitch, yaw); + calculate_euler_angles(m, &roll2, &pitch2, &yaw2); + if (m.is_nan()) { + Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n", + roll, pitch, yaw); + } + if (isnan(roll2) || + isnan(pitch2) || + isnan(yaw2)) { + Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n", + roll, pitch, yaw); + } + if (rad_diff(roll2,roll) > 0.01 || + rad_diff(pitch2, pitch) > 0.01 || + rad_diff(yaw2, yaw) > 0.01) { + Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", + roll, roll2, pitch, pitch2, yaw, yaw2); + } +} + +#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) + +static const float angles[] = { 0, PI/8, PI/4, PI/2, PI, + -PI/8, -PI/4, -PI/2, -PI}; + +void test_matrix_eulers(void) +{ + uint8_t i, j, k; + uint8_t N = ARRAY_LENGTH(angles); + + Serial.println("rotation matrix unit tests\n"); + + for (i=0; i ToRad(179)) { + // reverse all 3 + roll2 += fmod(roll2+PI, 2*PI); + pitch2 += fmod(pitch2+PI, 2*PI); + yaw2 += fmod(yaw2+PI, 2*PI); + } + + if (rad_diff(roll2,roll) > 0.01 || + rad_diff(pitch2, pitch) > 0.01 || + rad_diff(yaw2, yaw) > 0.01) { + if (ToDeg(rad_diff(pitch, PI/2)) < 1 || + ToDeg(rad_diff(pitch, -PI/2)) < 1) { + // we expect breakdown at these poles + Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", + ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); + } else { + Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", + ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); + } + } else { + Serial.printf("correct eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", + ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); + } +} + +void test_quaternion_eulers(void) +{ + uint8_t i, j, k; + uint8_t N = ARRAY_LENGTH(angles); + + Serial.println("quaternion unit tests\n"); + + test_quaternion(PI/4, 0, 0); + test_quaternion(0, PI/4, 0); + test_quaternion(0, 0, PI/4); + test_quaternion(-PI/4, 0, 0); + test_quaternion(0, -PI/4, 0); + test_quaternion(0, 0, -PI/4); + test_quaternion(-PI/4, 1, 1); + test_quaternion(1, -PI/4, 1); + test_quaternion(1, 1, -PI/4); + + test_quaternion(ToRad(89), 0, 0.1); + test_quaternion(0, ToRad(89), 0.1); + test_quaternion(0.1, 0, ToRad(89)); + + test_quaternion(ToRad(91), 0, 0.1); + test_quaternion(0, ToRad(91), 0.1); + test_quaternion(0.1, 0, ToRad(91)); + + for (i=0; i + +class Quaternion +{ +public: + float q1, q2, q3, q4; + + // constructor + Quaternion() { q1 = 1; q2 = q3 = q4 = 0; } + + // setting constructor + Quaternion(const float _q1, const float _q2, const float _q3, const float _q4): + q1(_q1), q2(_q2), q3(_q3), q4(_q4) {} + + // function call operator + void operator ()(const float _q1, const float _q2, const float _q3, const float _q4) + { q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; } + + // check if any elements are NAN + bool is_nan(void) + { return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); } + +}; +#endif // QUATERNION_H