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")
#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

2
libraries/AP_Math/quaternion.h
View File

@ -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>

7
libraries/AP_Math/rotations.h
View File

@ -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
*/

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";
#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
}

13
libraries/AP_Math/vector3.cpp
View File

@ -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