diff --git a/libraries/AP_Math/tests/test_rotations.cpp b/libraries/AP_Math/tests/test_rotations.cpp new file mode 100644 index 0000000000..15fdfa6238 --- /dev/null +++ b/libraries/AP_Math/tests/test_rotations.cpp @@ -0,0 +1,193 @@ +#include + +#include +#include + + +AP_CustomRotations cust_rot; +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + + +static void test_euler(enum Rotation rotation, float roll, float pitch, float yaw) +{ + Vector3f v, v1, v2, diff; + Matrix3f rotmat; + const float accuracy = 1.0e-6f; + + v.x = 1; + v.y = 2; + v.z = 3; + v1 = v; + + v1.rotate(rotation); + + rotmat.from_euler(radians(roll), radians(pitch), radians(yaw)); + v2 = v; + v2 = rotmat * v2; + + diff = (v2 - v1); + EXPECT_LE(diff.length(), accuracy); + + // quaternion rotation test + const float q_accuracy = 1.0e-3f; + Quaternion q, qe; + q.from_rotation(rotation); + qe.from_euler(radians(roll), radians(pitch), radians(yaw)); + float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw; + q.to_euler(q_roll, q_pitch, q_yaw); + qe.to_euler(qe_roll, qe_pitch, qe_yaw); + float roll_diff = fabsf(wrap_PI(q_roll - qe_roll)); + float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch)); + float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw)); + EXPECT_LE(roll_diff, q_accuracy); + EXPECT_LE(pitch_diff, q_accuracy); + EXPECT_LE(yaw_diff, q_accuracy); + + // test custom rotations + AP::custom_rotations().set(ROTATION_CUSTOM_1, roll, pitch, yaw); + v1 = v; + v1.rotate(ROTATION_CUSTOM_1); + + diff = (v2 - v1); + EXPECT_LE(diff.length(), accuracy); + + Quaternion qc; + qc.from_rotation(ROTATION_CUSTOM_1); + float qc_roll, qc_pitch, qc_yaw; + qc.to_euler(qc_roll, qc_pitch, qc_yaw); + roll_diff = fabsf(wrap_PI(qc_roll - qe_roll)); + pitch_diff = fabsf(wrap_PI(qc_pitch - qe_pitch)); + yaw_diff = fabsf(wrap_PI(qc_yaw - qe_yaw)); + EXPECT_LE(roll_diff, q_accuracy); + EXPECT_LE(pitch_diff, q_accuracy); + EXPECT_LE(yaw_diff, q_accuracy); +} + +TEST(RotationsTest, TestEulers) +{ + test_euler(ROTATION_NONE, 0, 0, 0); + test_euler(ROTATION_YAW_45, 0, 0, 45); + test_euler(ROTATION_YAW_90, 0, 0, 90); + test_euler(ROTATION_YAW_135, 0, 0, 135); + test_euler(ROTATION_YAW_180, 0, 0, 180); + test_euler(ROTATION_YAW_225, 0, 0, 225); + test_euler(ROTATION_YAW_270, 0, 0, 270); + test_euler(ROTATION_YAW_315, 0, 0, 315); + test_euler(ROTATION_ROLL_180, 180, 0, 0); + test_euler(ROTATION_ROLL_180_YAW_45, 180, 0, 45); + test_euler(ROTATION_ROLL_180_YAW_90, 180, 0, 90); + test_euler(ROTATION_ROLL_180_YAW_135, 180, 0, 135); + test_euler(ROTATION_PITCH_180, 0, 180, 0); + test_euler(ROTATION_ROLL_180_YAW_225, 180, 0, 225); + test_euler(ROTATION_ROLL_180_YAW_270, 180, 0, 270); + test_euler(ROTATION_ROLL_180_YAW_315, 180, 0, 315); + test_euler(ROTATION_ROLL_90, 90, 0, 0); + test_euler(ROTATION_ROLL_90_YAW_45, 90, 0, 45); + test_euler(ROTATION_ROLL_90_YAW_90, 90, 0, 90); + test_euler(ROTATION_ROLL_90_YAW_135, 90, 0, 135); + test_euler(ROTATION_ROLL_270, 270, 0, 0); + test_euler(ROTATION_ROLL_270_YAW_45, 270, 0, 45); + test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90); + test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135); + test_euler(ROTATION_PITCH_90, 0, 90, 0); + test_euler(ROTATION_PITCH_270, 0, 270, 0); + test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90); + test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270); + test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0); + test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0); + test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0); + test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0); + test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0); + test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0); + 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_PITCH_68_YAW_293,90,68.8,293.3); + test_euler(ROTATION_ROLL_45,45,0,0); + test_euler(ROTATION_ROLL_315,315,0,0); + test_euler(ROTATION_PITCH_7, 0, 7, 0); +} + + +TEST(RotationsTest, TestRotationInverse) +{ + // rotate inverse test(Vector (1,1,1)) + Vector3f vec(1.0f,1.0f,1.0f), cmp_vec(1.0f, 1.0f, 1.0f); + for (enum Rotation r = ROTATION_NONE; + r < ROTATION_MAX; + r = (enum Rotation)((uint8_t)r+1)) { + vec.rotate(r); + vec.rotate_inverse(r); + EXPECT_LE((vec - cmp_vec).length(), 1e-5); + } +} + +TEST(RotationsTest, TestRotateMatrix) +{ + for (enum Rotation r = ROTATION_NONE; + r < ROTATION_MAX; + r = (enum Rotation)((uint8_t)r+1)) { + Vector3f vec(1,2,3); + Vector3f vec2 = vec; + vec.rotate(r); + Matrix3f m; + m.from_rotation(r); + vec2 = m * vec2; + EXPECT_LE((vec - vec2).length(), 1e-5); + } +} + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +TEST(RotationsTest, TestFailedGetLinux) +{ + AP::custom_rotations().set(ROTATION_CUSTOM_OLD, 0, 0, 0); + Vector3f vec(1,2,3); + Vector3f vec2 = vec; + AP::custom_rotations().rotate(ROTATION_CUSTOM_OLD, vec2); + EXPECT_TRUE(vec == vec2); + Vector3d vecd(1,2,3); + Vector3d vecd2 = vecd; + AP::custom_rotations().rotate(ROTATION_CUSTOM_OLD, vecd2); + EXPECT_TRUE(vecd == vecd2); + Quaternion q(1.0f, 0.0f, 0.0f, 0.0f); + Quaternion q2(1.0f, 0.0f, 0.0f, 0.0f); + AP::custom_rotations().from_rotation(ROTATION_CUSTOM_OLD, q2); + for (int a = 0; a < 4; ++a) { + EXPECT_FLOAT_EQ(q[a], q2[a]); + } + QuaternionD qd(1.0, 0.0, 0.0, 0.0); + QuaternionD qd2(1.0, 0.0, 0.0, 0.0); + AP::custom_rotations().from_rotation(ROTATION_CUSTOM_OLD, qd2); + for (int a = 0; a < 4; ++a) { + EXPECT_FLOAT_EQ(qd[a], qd2[a]); + } +} +#endif + +static void breakSingleton() +{ + AP_CustomRotations cust_rot1; +} +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +TEST(RotationsTest, TestFailedGetLinux) +{ + EXPECT_EXIT(AP::custom_rotations().set(ROTATION_CUSTOM_OLD, 0, 0, 0), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation"); + EXPECT_EXIT(AP::custom_rotations().set(ROTATION_CUSTOM_END, 0, 0, 0), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation"); + EXPECT_EXIT(breakSingleton(), testing::KilledBySignal(SIGABRT), "AP_CustomRotations must be singleton"); +} +#endif + +/*TEST(RotationsTest, TestRotateEqual) +{ + for (enum Rotation r = (enum Rotation)((uint8_t)ROTATION_MAX-1); r > ROTATION_NONE; r = (enum Rotation)((uint8_t)r-1)) { + for (enum Rotation r2 = ROTATION_NONE; r2 < r; r2 = (enum Rotation)((uint8_t)r2+1)) { + if (rotation_equal(r,r2)) { + hal.console->printf("Rotation %i same as %i\n", r, r2); + } + } + } +}*/ + +AP_GTEST_PANIC() +AP_GTEST_MAIN()