AP_Math: added new rotation for PITCH_7

libraries/AP_Math/examples/rotations/rotations.cpp

@@ -220,6 +220,7 @@ static void test_eulers(void)
     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_PITCH_7, 0, 7, 0);
 }
 
 static bool have_rotation(const Matrix3f &m)

libraries/AP_Math/rotations.h

@@ -66,6 +66,7 @@ enum Rotation : uint8_t {
     ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
     ROTATION_PITCH_315           = 39,
     ROTATION_ROLL_90_PITCH_315   = 40,
+    ROTATION_PITCH_7             = 41,
     ///////////////////////////////////////////////////////////////////////
     // Do not add more rotations without checking that there is not a conflict
     // with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our

libraries/AP_Math/vector3.cpp

@@ -244,6 +244,15 @@ void Vector3<T>::rotate(enum Rotation rotation)
         x = tmp;
         return;
     }
+    case ROTATION_PITCH_7: {
+        const float sin_pitch = 0.12186934340514748f; // sinf(pitch);
+        const float cos_pitch = 0.992546151641322f; // cosf(pitch);
+        float tmpx = x;
+        float tmpz = z;
+        x =  cos_pitch * tmpx + sin_pitch * tmpz;
+        z = -sin_pitch * tmpx + cos_pitch * tmpz;
+        return;
+    }
     case ROTATION_CUSTOM: // no-op; caller should perform custom rotations via matrix multiplication
         return;
     }