AP_Math: added from_rotation() method to Matrix3

this is used to get a rotation matrix from a rotation enum

libraries/AP_Math/examples/rotations/rotations.cpp

@@ -253,16 +253,41 @@ static void missing_rotations(void)
             }
 }
 
+static void test_rotate_matrix(void)
+{
+    for (enum Rotation r = ROTATION_NONE;
+         r < ROTATION_MAX;
+         r = (enum Rotation)((uint8_t)r+1)) {
+        //hal.console->printf("\nROTATION(%d)\n", r);
+        Vector3f vec(1,2,3);
+        Vector3f vec2 = vec;
+        vec.rotate(r);
+        Matrix3f m;
+        m.from_rotation(r);
+        vec2 = m * vec2;
+        //print_vector(vec);
+        //print_vector(vec2);
+        if ((vec - vec2).length() > 1e-5) {
+            hal.console->printf("Rotation Test Failed!!! %.8f\n", (double)(vec - vec2).length());
+            return;
+        }
+    }
+    hal.console->printf("test_rotate_matrix passed\n");
+}
+
+
 /*
  *  rotation tests
  */
 void setup(void)
 {
+    hal.console->begin(115200);
     hal.console->printf("rotation unit tests\n\n");
     test_rotation_accuracy();
     test_eulers();
     missing_rotations();
     test_rotate_inverse();
+    test_rotate_matrix();
     hal.console->printf("rotation unit tests done\n\n");
 }
 

libraries/AP_Math/matrix3.cpp

@@ -59,6 +59,19 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
     }
 }
 
+template <typename T>
+void Matrix3<T>::from_rotation(enum Rotation rotation)
+{
+    (*this).a(1,0,0);
+    (*this).b(0,1,0);
+    (*this).c(0,0,1);
+
+    (*this).a.rotate(rotation);
+    (*this).b.rotate(rotation);
+    (*this).c.rotate(rotation);
+    (*this).transpose();
+}
+
 /*
   calculate Euler angles (312 convention) for the matrix.
   See http://www.atacolorado.com/eulersequences.doc
@@ -260,6 +273,7 @@ template void Matrix3<float>::rotate(const Vector3<float> &g);
 template void Matrix3<float>::normalize(void);
 template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
 template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw) const;
+template void Matrix3<float>::from_rotation(enum Rotation rotation);
 template void Matrix3<float>::from_euler312(float roll, float pitch, float yaw);
 template void Matrix3<float>::from_axis_angle(const Vector3<float> &v, float theta);
 template Vector3<float> Matrix3<float>::to_euler312(void) const;

libraries/AP_Math/matrix3.h

@@ -235,6 +235,9 @@ public:
     // create eulers from a rotation matrix
     void        to_euler(float *roll, float *pitch, float *yaw) const;
 
+    // create matrix from rotation enum
+    void from_rotation(enum Rotation rotation);
+    
     /*
       calculate Euler angles (312 convention) for the matrix.
       See http://www.atacolorado.com/eulersequences.doc