AP_Math: add support for AP_CustomRotations

libraries/AP_Math/quaternion.cpp

@@ -18,8 +18,11 @@
 
 #pragma GCC optimize("O2")
 
+#include "quaternion.h"
 #include "AP_Math.h"
 #include <AP_InternalError/AP_InternalError.h>
+#include <AP_CustomRotations/AP_CustomRotations.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 
 #define HALF_SQRT_2_PlUS_SQRT_2 0.92387953251128673848313610506011 // sqrt(2 + sqrt(2)) / 2
 #define HALF_SQRT_2_MINUS_SQTR_2 0.38268343236508972626808144923416 // sqrt(2 - sqrt(2)) / 2
@@ -376,12 +379,16 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
         q3 = q4 = 0.0;
         return;
 
-    case ROTATION_CUSTOM:
-        // Error; custom rotations not supported
-        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    case ROTATION_CUSTOM_1:
+    case ROTATION_CUSTOM_2:
+#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
+        // Do not support custom rotations on Periph
+        AP::custom_rotations().from_rotation(rotation, *this);
         return;
-
+#endif
     case ROTATION_MAX:
+    case ROTATION_CUSTOM_OLD:
+    case ROTATION_CUSTOM_END:
         break;
     }
     // rotation invalid

libraries/AP_Math/quaternion.h

@@ -17,6 +17,8 @@
 // Refactored by Jonathan Challinger
 #pragma once
 
+#include "definitions.h"
+#include "matrix3.h"
 #include <cmath>
 #if MATH_CHECK_INDEXES
 #include <assert.h>

libraries/AP_Math/rotations.h

@@ -76,7 +76,10 @@ enum Rotation : uint8_t {
     // to the MAVLink messages as well.
     ///////////////////////////////////////////////////////////////////////
     ROTATION_MAX,
-    ROTATION_CUSTOM              = 100,
+    ROTATION_CUSTOM_OLD          = 100,
+    ROTATION_CUSTOM_1            = 101,
+    ROTATION_CUSTOM_2            = 102,
+    ROTATION_CUSTOM_END,
 };
 
 
@@ -87,5 +90,5 @@ enum Rotation : uint8_t {
 Here are the same values in a form suitable 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:Roll270Yaw135,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,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,42:Roll45,43:Roll315,100:Custom
+@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
  */

libraries/AP_Math/tests/test_math.cpp

@@ -83,11 +83,11 @@ TEST(VectorTest, Rotations)
     EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-    TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1);
+    TEST_ROTATION(ROTATION_CUSTOM_OLD, 1, 1, 1);
     TEST_ROTATION(ROTATION_MAX, 1, 1, 1);
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
     Vector3F v {1, 1, 1};
-    EXPECT_EXIT(v.rotate(ROTATION_CUSTOM), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::flow_of_ctrl");
+    EXPECT_EXIT(v.rotate(ROTATION_CUSTOM_OLD), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation");
     EXPECT_EXIT(v.rotate(ROTATION_MAX), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation");
 #endif
 }

libraries/AP_Math/vector3.cpp

@@ -20,6 +20,8 @@
 
 #include "AP_Math.h"
 #include <AP_InternalError/AP_InternalError.h>
+#include <AP_CustomRotations/AP_CustomRotations.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 
 // rotate a vector by a standard rotation, attempting
 // to use the minimum number of floating point operations
@@ -255,11 +257,16 @@ void Vector3<T>::rotate(enum Rotation rotation)
         y = tmp;
         return;
     }
-    case ROTATION_CUSTOM: 
-        // Error: caller must perform custom rotations via matrix multiplication
-        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    case ROTATION_CUSTOM_1:
+    case ROTATION_CUSTOM_2:
+#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
+        // Do not support custom rotations on Periph
+        AP::custom_rotations().rotate(rotation, *this);
         return;
+#endif
     case ROTATION_MAX:
+    case ROTATION_CUSTOM_OLD:
+    case ROTATION_CUSTOM_END:
         break;
     }
     // rotation invalid