From 4c752e4a94101db714a97fb8512694ffa8c856c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 19:17:36 +1000 Subject: [PATCH] AP_Math: simplify rotations test code --- .../AP_Math/examples/rotations/rotations.pde | 183 ------------------ 1 file changed, 183 deletions(-) diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 070a071c36..9d4c9a31d1 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -22,186 +22,12 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -// standard rotation matrices (these are the originals from the old code) -#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0,0, 1) -#define MATRIX_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define MATRIX_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) -#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) -#define MATRIX_ROTATION_ROLL_90 Matrix3f(1, 0, 0, 0, 0, -1, 0, 1, 0) -#define MATRIX_ROTATION_ROLL_270 Matrix3f(1, 0, 0, 0, 0, 1, 0, -1, 0) -#define MATRIX_ROTATION_PITCH_90 Matrix3f(0, 0, 1, 0, 1, 0, -1, 0, 0) -#define MATRIX_ROTATION_PITCH_270 Matrix3f(0, 0, -1, 0, 1, 0, 1, 0, 0) - -static void print_matrix(Matrix3f &m) -{ - hal.console->printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n", - m.a.x, m.a.y, m.a.z, - m.b.x, m.b.y, m.b.z, - m.c.x, m.c.y, m.c.z); -} - static void print_vector(Vector3f &v) { hal.console->printf("[%.2f %.2f %.2f]\n", v.x, v.y, v.z); } -// test one matrix -static void test_matrix(enum Rotation rotation, Matrix3f m) -{ - Matrix3f m2, diff; - const float accuracy = 1.0e-6; - m2.rotation(rotation); - diff = (m - m2); - if (diff.a.length() > accuracy || - diff.b.length() > accuracy || - diff.c.length() > accuracy) { - hal.console->printf("rotation matrix %u incorrect\n", (unsigned)rotation); - print_matrix(m); - print_matrix(m2); - } -} - -// test generation of rotation matrices -static void test_matrices(void) -{ - hal.console->println("testing rotation matrices\n"); - test_matrix(ROTATION_NONE, MATRIX_ROTATION_NONE); - test_matrix(ROTATION_YAW_45, MATRIX_ROTATION_YAW_45); - test_matrix(ROTATION_YAW_90, MATRIX_ROTATION_YAW_90); - test_matrix(ROTATION_YAW_135, MATRIX_ROTATION_YAW_135); - test_matrix(ROTATION_YAW_180, MATRIX_ROTATION_YAW_180); - test_matrix(ROTATION_YAW_225, MATRIX_ROTATION_YAW_225); - test_matrix(ROTATION_YAW_270, MATRIX_ROTATION_YAW_270); - test_matrix(ROTATION_YAW_315, MATRIX_ROTATION_YAW_315); - test_matrix(ROTATION_ROLL_180, MATRIX_ROTATION_ROLL_180); - test_matrix(ROTATION_ROLL_180_YAW_45, MATRIX_ROTATION_ROLL_180_YAW_45); - test_matrix(ROTATION_ROLL_180_YAW_90, MATRIX_ROTATION_ROLL_180_YAW_90); - test_matrix(ROTATION_ROLL_180_YAW_135, MATRIX_ROTATION_ROLL_180_YAW_135); - test_matrix(ROTATION_PITCH_180, MATRIX_ROTATION_PITCH_180); - test_matrix(ROTATION_ROLL_180_YAW_225, MATRIX_ROTATION_ROLL_180_YAW_225); - test_matrix(ROTATION_ROLL_180_YAW_270, MATRIX_ROTATION_ROLL_180_YAW_270); - test_matrix(ROTATION_ROLL_180_YAW_315, MATRIX_ROTATION_ROLL_180_YAW_315); - test_matrix(ROTATION_ROLL_90, MATRIX_ROTATION_ROLL_90); - test_matrix(ROTATION_ROLL_270, MATRIX_ROTATION_ROLL_270); - test_matrix(ROTATION_PITCH_90, MATRIX_ROTATION_PITCH_90); - test_matrix(ROTATION_PITCH_270, MATRIX_ROTATION_PITCH_270); -} - -// test rotation of vectors -static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true) -{ - Vector3f v2, diff; - Matrix3f m; - v2 = v1; - m.rotation(rotation); - v1.rotate(rotation); - v2 = m * v2; - diff = v1 - v2; - if (diff.length() > 1.0e-6) { - hal.console->printf("rotation vector %u incorrect\n", (unsigned)rotation); - hal.console->printf("%u %f %f %f\n", - (unsigned)rotation, - v2.x, v2.y, v2.z); - } - if (show) { - hal.console->printf("%u %f %f %f\n", - (unsigned)rotation, - v1.x, v1.y, v1.z); - } -} - -// generate a random float between -1 and 1 -static float rand_num(void) -{ - float ret = ((unsigned)random()) % 2000000; - return (ret - 1.0e6) / 1.0e6; -} - -// test rotation of vectors -static void test_vector(enum Rotation rotation) -{ - uint8_t i; - - Vector3f v1; - v1.x = 1; - v1.y = 2; - v1.z = 3; - test_vector(rotation, v1); - - for (i=0; i<10; i++) { - v1.x = rand_num(); - v1.y = rand_num(); - v1.z = rand_num(); - test_vector(rotation, v1, false); - } -} - -// test rotation of vectors -static void test_vectors(void) -{ - hal.console->println("testing rotation of vectors\n"); - test_vector(ROTATION_NONE); - test_vector(ROTATION_YAW_45); - test_vector(ROTATION_YAW_90); - test_vector(ROTATION_YAW_135); - test_vector(ROTATION_YAW_180); - test_vector(ROTATION_YAW_225); - test_vector(ROTATION_YAW_270); - test_vector(ROTATION_YAW_315); - test_vector(ROTATION_ROLL_180); - test_vector(ROTATION_ROLL_180_YAW_45); - test_vector(ROTATION_ROLL_180_YAW_90); - test_vector(ROTATION_ROLL_180_YAW_135); - test_vector(ROTATION_PITCH_180); - test_vector(ROTATION_ROLL_180_YAW_225); - test_vector(ROTATION_ROLL_180_YAW_270); - test_vector(ROTATION_ROLL_180_YAW_315); -} - - -static void new_combination(enum Rotation r1, enum Rotation r2) -{ - -} - -#if ROTATION_COMBINATION_SUPPORT -// test combinations of rotations -static void test_combinations(void) -{ - enum Rotation r1, r2, r3; - bool found; - - for (r1=ROTATION_NONE; r1printf("rotation: %u + %u -> %u\n", - (unsigned)r1, (unsigned)r2, (unsigned)r3); - } else { - hal.console->printf("ERROR rotation: no combination for %u + %u\n", - (unsigned)r1, (unsigned)r2); - new_combination(r1, r2); - } - } - } -} -#endif - // test rotation method accuracy static void test_rotation_accuracy(void) @@ -260,10 +86,6 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya print_vector(v1); print_vector(v2); } -#if 0 - if (rotation >= ROTATION_ROLL_90_YAW_45) - print_matrix(rotmat); -#endif } static void test_eulers(void) @@ -347,11 +169,6 @@ static void missing_rotations(void) void setup(void) { hal.console->println("rotation unit tests\n"); - test_matrices(); - test_vectors(); -#if ROTATION_COMBINATION_SUPPORT - test_combinations(); -#endif test_rotation_accuracy(); test_eulers(); missing_rotations();