AP_Math: added missing 90 degree rotations

this ensures we can handle all 90 degree rotations of the compass and
main board. A test in examples/rotations shows that we have them all.

libraries/AP_Math/examples/rotations/rotations.pde

@@ -295,6 +295,50 @@ static void test_eulers(void)
     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);    
+}
+
+static bool have_rotation(const Matrix3f &m)
+{
+    Matrix3f mt = m.transposed();
+    for (enum Rotation r=ROTATION_NONE; 
+         r<ROTATION_MAX;
+         r = (enum Rotation)((uint8_t)r+1)) {
+        Vector3f v(1,2,3);
+        Vector3f v2 = v;
+        v2.rotate(r);
+        v2 = mt * v2;
+        if ((v2 - v).length() < 0.01f) {
+            return true;
+        }
+    }
+    return false;
+}
+
+static void missing_rotations(void)
+{
+    hal.console->println("testing for missing rotations");
+    uint16_t roll, pitch, yaw;
+    for (yaw=0; yaw<360; yaw += 90)
+        for (pitch=0; pitch<360; pitch += 90)
+            for (roll=0; roll<360; roll += 90) {
+                Matrix3f m;
+                m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw));
+                if (!have_rotation(m)) {
+                    hal.console->printf("Missing rotation (%u, %u, %u)\n", roll, pitch, yaw);
+                }
+            }
 }
 
 /*
@@ -310,6 +354,7 @@ void setup(void)
 #endif
     test_rotation_accuracy();
     test_eulers();
+    missing_rotations();
     hal.console->println("rotation unit tests done\n");
 }
 

libraries/AP_Math/rotations.h

@@ -52,11 +52,23 @@ enum Rotation {
     ROTATION_ROLL_270_YAW_135    = 23,
     ROTATION_PITCH_90            = 24,
     ROTATION_PITCH_270           = 25,
+    ROTATION_PITCH_180_YAW_90    = 26,
+    ROTATION_PITCH_180_YAW_270   = 27,
+    ROTATION_ROLL_90_PITCH_90    = 28,
+    ROTATION_ROLL_180_PITCH_90   = 29,
+    ROTATION_ROLL_270_PITCH_90   = 30,
+    ROTATION_ROLL_90_PITCH_180   = 31,
+    ROTATION_ROLL_270_PITCH_180  = 32,
+    ROTATION_ROLL_90_PITCH_270   = 33,
+    ROTATION_ROLL_180_PITCH_270  = 34,
+    ROTATION_ROLL_270_PITCH_270  = 35,
+    ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
+    ROTATION_ROLL_90_YAW_270     = 37,
     ROTATION_MAX
 };
 /*
 Here are the same values in a form sutable for a @Values attribute in
 auto documentation:
 
-@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270
+@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270
  */

libraries/AP_Math/vector3.cpp

@@ -160,6 +160,67 @@ void Vector3<T>::rotate(enum Rotation rotation)
         tmp = z; z = x; x = -tmp;
         return;
     }
+    case ROTATION_PITCH_180_YAW_90: {
+        z = -z;
+        tmp = -x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_PITCH_180_YAW_270: {
+        x = -x; z = -z;
+        tmp = x; x = y; y = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_PITCH_90: {
+        tmp = z; z = y; y = -tmp;
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_180_PITCH_90: {
+        y = -y; z = -z;
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_PITCH_90: {
+        tmp = z; z = -y; y = tmp;
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_PITCH_180: {
+        tmp = z; z = y; y = -tmp;
+        x = -x; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_270_PITCH_180: {
+        tmp = z; z = -y; y = tmp;
+        x = -x; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_90_PITCH_270: {
+        tmp = z; z = y; y = -tmp;
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_180_PITCH_270: {
+        y = -y; z = -z;
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_PITCH_270: {
+        tmp = z; z = -y; y = tmp;
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_PITCH_180_YAW_90: {
+        tmp = z; z = y; y = -tmp;
+        x = -x; z = -z;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_270: {
+        tmp = z; z = y; y = -tmp;
+        tmp = x; x = y; y = -tmp;
+        return;
+    }
     }
 }