From 7a6f57ccf1c8d593c346e214829b1e1bc417f96b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:12:24 +0000 Subject: [PATCH] AP_Math: add support for AP_CustomRotations --- libraries/AP_Math/quaternion.cpp | 15 +++++++++++---- libraries/AP_Math/quaternion.h | 2 ++ libraries/AP_Math/rotations.h | 7 +++++-- libraries/AP_Math/tests/test_math.cpp | 4 ++-- libraries/AP_Math/vector3.cpp | 13 ++++++++++--- 5 files changed, 30 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 93918818b5..99d9c0824e 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -18,8 +18,11 @@ #pragma GCC optimize("O2") +#include "quaternion.h" #include "AP_Math.h" #include +#include +#include #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::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 diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index e73eeaa297..ad09ef583b 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -17,6 +17,8 @@ // Refactored by Jonathan Challinger #pragma once +#include "definitions.h" +#include "matrix3.h" #include #if MATH_CHECK_INDEXES #include diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index d7b4dabbba..042ac09033 100644 --- a/libraries/AP_Math/rotations.h +++ b/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 */ diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index bdd4e2ec4f..775f21b7ae 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/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 } diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 95864cca7f..e13ab69a27 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -20,6 +20,8 @@ #include "AP_Math.h" #include +#include +#include // rotate a vector by a standard rotation, attempting // to use the minimum number of floating point operations @@ -255,11 +257,16 @@ void Vector3::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