From 503d14428dae343729a0461d4fa55ad54ffbb71b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 22 Nov 2014 14:10:00 +0900 Subject: [PATCH] AP_Math: add new rotation to example rotation sketch --- .../AP_Math/examples/rotations/rotations.pde | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 017e0ea6c0..2327bd9a51 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -37,11 +37,10 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static void print_vector(Vector3f &v) { - hal.console->printf("[%.2f %.2f %.2f]\n", + hal.console->printf("[%.4f %.4f %.4f]\n", v.x, v.y, v.z); } - // test rotation method accuracy static void test_rotation_accuracy(void) { @@ -94,10 +93,16 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya diff = (v2 - v1); if (diff.length() > accuracy) { - hal.console->printf("euler test %u incorrect\n", (unsigned)rotation); - print_vector(v); + hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n", + (unsigned)rotation, + (int)yaw, + (int)roll, + (int)pitch); + hal.console->printf("fast rotated: "); print_vector(v1); + hal.console->printf("slow rotated: "); print_vector(v2); + hal.console->printf("\n"); } } @@ -141,7 +146,8 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0); test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0); test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90); - test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); + test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); + test_euler(ROTATION_YAW_293_PITCH_68_ROLL_180,180,68.8,293.3); } static bool have_rotation(const Matrix3f &m)