AP_Math: added comments and a test for euler ordering

our main euler functions did not have a comment on the ordering
convention

libraries/AP_Math/matrix3.h

@@ -235,13 +235,17 @@ public:
         return a.is_nan() || b.is_nan() || c.is_nan();
     }
 
-    // create a rotation matrix from Euler angles
+    /*
+      create a rotation matrix from Euler angles in 321 euler orderin
+    */
     void        from_euler(T roll, T pitch, T yaw);
 
-    // create eulers from a rotation matrix.
-    // roll is from -Pi to Pi
-    // pitch is from -Pi/2 to Pi/2
-    // yaw is from -Pi to Pi
+    /* create eulers from a rotation matrix.
+       roll is from -Pi to Pi
+       pitch is from -Pi/2 to Pi/2
+       yaw is from -Pi to Pi
+       euler order is 321
+    */
     void        to_euler(T *roll, T *pitch, T *yaw) const;
 
     // create matrix from rotation enum

libraries/AP_Math/quaternion.h

@@ -81,11 +81,11 @@ public:
     // convert a vector from earth to body frame
     void        earth_to_body(Vector3<T> &v) const;
 
-    // create a quaternion from Euler angles
+    // create a quaternion from Euler angles using 321 euler ordering
     void        from_euler(T roll, T pitch, T yaw);
     void        from_euler(const Vector3<T> &v);
 
-    // create a quaternion from Euler angles applied in yaw, roll, pitch order
+    // create a quaternion from Euler angles applied in yaw, roll, pitch order (312)
     // instead of the normal yaw, pitch, roll order
     void        from_vector312(T roll, T pitch, T yaw);
 
@@ -129,7 +129,7 @@ public:
     // get euler yaw angle in radians
     T       get_euler_yaw() const;
 
-    // create eulers (in radians) from a quaternion
+    // create eulers (in radians) from a quaternion, using 321 ordering
     void        to_euler(float &roll, float &pitch, float &yaw) const;
     void        to_euler(Vector3f &rpy) const {
         to_euler(rpy.x, rpy.y, rpy.z);
@@ -139,7 +139,7 @@ public:
         to_euler(rpy.x, rpy.y, rpy.z);
     }
 
-    // create eulers from a quaternion
+    // create eulers from a quaternion with 312 ordering
     Vector3<T>    to_vector312(void) const;
 
     T length_squared(void) const;

libraries/AP_Math/tests/test_rotations.cpp

@@ -189,5 +189,83 @@ TEST(RotationsTest, TestFailedGetLinux)
     }
 }*/
 
+/*
+  rotate a matrix using a give order, specified as a string
+  for example "321"
+ */
+static void rotate_ordered(Matrix3f &m, const char *order,
+                           const float roll_deg,
+                           const float pitch_deg,
+                           const float yaw_deg)
+{
+    while (*order) {
+        Matrix3f m2;
+        switch (*order) {
+        case '1':
+            m2.from_euler(radians(roll_deg), 0, 0);
+            break;
+        case '2':
+            m2.from_euler(0, radians(pitch_deg), 0);
+            break;
+        case '3':
+            m2.from_euler(0, 0, radians(yaw_deg));
+            break;
+        }
+        m *= m2;
+        order++;
+    }
+}
+
+/*
+  test the two euler orders we use in ArduPilot
+ */
+TEST(RotationsTest, TestEulerOrder)
+{
+    const float roll_deg = 20;
+    const float pitch_deg = 31;
+    const float yaw_deg = 72;
+    float r, p, y;
+    Vector3f v;
+
+    Matrix3f m;
+
+    // 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);
+
+    // get using to_euler312, should not match
+    v = m.to_euler312();
+
+    EXPECT_GE(fabsf(degrees(v.x)-roll_deg), 1);
+    EXPECT_GE(fabsf(degrees(v.y)-pitch_deg), 1);
+    EXPECT_GE(fabsf(degrees(v.z)-yaw_deg), 1);
+
+    // apply in 312 ordering
+    m.identity();
+    rotate_ordered(m, "312", roll_deg, pitch_deg, yaw_deg);
+
+    // get using to_euler312
+    v = m.to_euler312();
+
+    EXPECT_FLOAT_EQ(degrees(v.x), roll_deg);
+    EXPECT_FLOAT_EQ(degrees(v.y), pitch_deg);
+    EXPECT_FLOAT_EQ(degrees(v.z), yaw_deg);
+
+    // get using to_euler, should not match
+    m.to_euler(&r, &p, &y);
+
+    EXPECT_GE(fabsf(degrees(r)-roll_deg), 1);
+    EXPECT_GE(fabsf(degrees(p)-pitch_deg), 1);
+    EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1);
+}
+
+
 AP_GTEST_PANIC()
 AP_GTEST_MAIN()