AP_Math: add TestEulerOrderRoll90Yaw90

libraries/AP_Math/tests/test_rotations.cpp

@@ -165,11 +165,12 @@ TEST(RotationsTest, TestFailedGetLinux)
 }
 #endif
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 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");
@@ -266,6 +267,45 @@ TEST(RotationsTest, TestEulerOrder)
     EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1);
 }
 
+/*
+  test the two euler orders we use in ArduPilot
+ */
+TEST(RotationsTest, TestEulerOrderRoll90Yaw90)
+{
+    const float roll_deg = 90;
+    const float pitch_deg = 0;
+    const float yaw_deg = 90;
+    float r, p, y;
+    Vector3f v(1.0f,0.0f,0.0f);;
+
+    Matrix3f m, m2;
+    m2.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+    // apply in 321 ordering
+    m.identity();
+    rotate_ordered(m, "321", roll_deg, pitch_deg, yaw_deg);
+
+    // get using to_euler
+    m.to_euler(&r, &p, &y);
+
+    EXPECT_FLOAT_EQ(degrees(r), roll_deg);
+    EXPECT_FLOAT_EQ(degrees(p), pitch_deg);
+    EXPECT_FLOAT_EQ(degrees(y), yaw_deg);
+
+    Vector3f v1 = m * v;
+    Vector3f v2 = v;
+
+    v2.rotate(ROTATION_ROLL_90_YAW_90);
+
+    EXPECT_FLOAT_EQ(v1.x, v2.x);
+    EXPECT_FLOAT_EQ(v1.y, v2.y);
+    EXPECT_FLOAT_EQ(v1.z, v2.z);
+
+    v1 = m2 * v;
+    EXPECT_FLOAT_EQ(v1.x, v2.x);
+    EXPECT_FLOAT_EQ(v1.y, v2.y);
+    EXPECT_FLOAT_EQ(v1.z, v2.z);
+}
 
 AP_GTEST_PANIC()
 AP_GTEST_MAIN()