From c18fce6714eba1b9fab454e9c2a41033fbbe4d45 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Apr 2020 14:53:05 +0900 Subject: [PATCH] AP_Math: add quaternion rotation test --- .../AP_Math/examples/rotations/rotations.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/libraries/AP_Math/examples/rotations/rotations.cpp b/libraries/AP_Math/examples/rotations/rotations.cpp index 4b60fecfb2..989bec4aed 100644 --- a/libraries/AP_Math/examples/rotations/rotations.cpp +++ b/libraries/AP_Math/examples/rotations/rotations.cpp @@ -156,6 +156,25 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya print_vector(v2); hal.console->printf("\n"); } + + // quaternion rotation test + Quaternion q; + q.from_rotation(rotation); + float q_roll, q_pitch, q_yaw; + q.to_euler(q_roll, q_pitch, q_yaw); + q_roll = degrees(wrap_2PI(q_roll)); + q_pitch = degrees(wrap_2PI(q_pitch)); + q_yaw = degrees(wrap_2PI(q_yaw)); + if ((fabsf(q_roll - roll) > accuracy) || (fabsf(q_pitch - pitch) > accuracy) || (fabsf(q_yaw - yaw) > accuracy)) { + hal.console->printf("quaternion test %u failed : yaw:%d roll:%d pitch:%d vs yaw:%f roll:%f pitch:%f\n", + (unsigned)rotation, + (int)yaw, + (int)roll, + (int)pitch, + (double)q_yaw, + (double)q_roll, + (double)q_pitch); + } } static void test_rotate_inverse(void)