From a92fb67b708f00f297bf7140433545491b86d2a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Mar 2012 22:42:29 +1100 Subject: [PATCH] AP_Math: expanded the math test suite --- libraries/AP_Math/examples/eulers/Makefile | 3 + libraries/AP_Math/examples/eulers/eulers.pde | 154 +++++++++++++------ 2 files changed, 110 insertions(+), 47 deletions(-) diff --git a/libraries/AP_Math/examples/eulers/Makefile b/libraries/AP_Math/examples/eulers/Makefile index d1f40fd90f..fcdc8ff8fe 100644 --- a/libraries/AP_Math/examples/eulers/Makefile +++ b/libraries/AP_Math/examples/eulers/Makefile @@ -1 +1,4 @@ include ../../../AP_Common/Arduino.mk + +sitl: + make -f ../../../../libraries/Desktop/Desktop.mk diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 2f27d98b9e..5db00ec80e 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -9,6 +9,22 @@ FastSerialPort(Serial, 0); +#ifdef DESKTOP_BUILD +// all of this is needed to build with SITL +#include +#include +#include +#include +#include +#include +#include +#include +#include +Arduino_Mega_ISR_Registry isr_registry; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; +#endif + static float rad_diff(float rad1, float rad2) { float diff = rad1 - rad2; @@ -21,31 +37,51 @@ static float rad_diff(float rad1, float rad2) return fabs(diff); } -static void test_euler(float roll, float pitch, float yaw) +static void check_result(float roll, float pitch, float yaw, + float roll2, float pitch2, float yaw2) { - 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) > 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) { - Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", - roll, roll2, pitch, pitch2, yaw, yaw2); + 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)); } } +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); + check_result(roll, pitch, yaw, roll2, pitch2, yaw2); +} + #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) static const float angles[] = { 0, PI/8, PI/4, PI/2, PI, @@ -73,40 +109,7 @@ static void test_quaternion(float roll, float pitch, float yaw) quaternion_from_euler(q, roll, pitch, yaw); euler_from_quaternion(q, &roll2, &pitch2, &yaw2); - if (q.is_nan()) { - Serial.printf("NAN quaternion 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) > 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)); - } + check_result(roll, pitch, yaw, roll2, pitch2, yaw2); } void test_quaternion_eulers(void) @@ -142,6 +145,61 @@ void test_quaternion_eulers(void) Serial.println("tests done\n"); } + +static void test_conversion(float roll, float pitch, float yaw) +{ + Quaternion q; + Matrix3f m, m2; + + float roll2, pitch2, yaw2; + + quaternion_from_euler(q, roll, pitch, yaw); + quaternion_to_rotation_matrix(q, m); + rotation_matrix_from_euler(m2, 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); + } + check_result(roll, pitch, yaw, roll2, pitch2, yaw2); +} + +void test_conversions(void) +{ + uint8_t i, j, k; + uint8_t N = ARRAY_LENGTH(angles); + + Serial.println("matrix/quaternion tests\n"); + + test_conversion(1, 1.1, 1.2); + test_conversion(1, -1.1, 1.2); + test_conversion(1, -1.1, -1.2); + test_conversion(-1, 1.1, -1.2); + test_conversion(-1, 1.1, 1.2); + + for (i=0; i