aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: add support for AP_CustomRotations

Browse Source
This commit is contained in:
Iampete1 2021-11-05 16:12:24 +00:00 committed by Andrew Tridgell
parent b77476caa1
commit 7a6f57ccf1
5 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Math/quaternion.cpp
View File

@ -18,8 +18,11 @@
#pragma GCC optimize("O2") #pragma GCC optimize("O2")
#include "quaternion.h"
#include "AP_Math.h" #include "AP_Math.h"
#include <AP_InternalError/AP_InternalError.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_PlUS_SQRT_2 0.92387953251128673848313610506011 // sqrt(2 + sqrt(2)) / 2
#define HALF_SQRT_2_MINUS_SQTR_2 0.38268343236508972626808144923416 // 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; q3 = q4 = 0.0;
return; return;
case ROTATION_CUSTOM: case ROTATION_CUSTOM_1:
// Error; custom rotations not supported case ROTATION_CUSTOM_2:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
// Do not support custom rotations on Periph
AP::custom_rotations().from_rotation(rotation, *this);
return; return;
#endif
case ROTATION_MAX: case ROTATION_MAX:
case ROTATION_CUSTOM_OLD:
case ROTATION_CUSTOM_END:
break; break;
} }
// rotation invalid // rotation invalid

2
libraries/AP_Math/quaternion.h
View File

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

7
libraries/AP_Math/rotations.h
View File

@ -76,7 +76,10 @@ enum Rotation : uint8_t {
// to the MAVLink messages as well. // to the MAVLink messages as well.
/////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////
ROTATION_MAX, 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 Here are the same values in a form suitable for a @Values attribute in
auto documentation: 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
*/ */

4
libraries/AP_Math/tests/test_math.cpp
View File

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

13
libraries/AP_Math/vector3.cpp
View File

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