Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: move to 32 digit rotation constants

Browse Source
This commit is contained in:
Iampete1 2021-07-23 23:09:46 +01:00 committed by Andrew Tridgell
parent 78e025af39
commit 22100dd37c
4 changed files with 61 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
libraries/AP_Math/quaternion.cpp
View File

@ -21,6 +21,11 @@
#include "AP_Math.h"
#include <AP_InternalError/AP_InternalError.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
#define HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO 0.65328148243818828788676000840496 // sqrt((2 + sqrt(2))/2) / 2
#define HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO 0.27059805007309845059637609665515 // sqrt((2 - sqrt(2))/2) / 2
// return the rotation matrix equivalent for this quaternion
template <typename T>
void QuaternionT<T>::rotation_matrix(Matrix3d &m) const
@ -137,9 +142,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_YAW_45:
q1 = 0.92387956f;
q1 = HALF_SQRT_2_PlUS_SQRT_2;
q2 = q3 = 0;
q4 = 0.38268343f;
q4 = HALF_SQRT_2_MINUS_SQTR_2;
return;
case ROTATION_YAW_90:
@ -149,9 +154,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_YAW_135:
q1 = 0.38268343f;
q1 = HALF_SQRT_2_MINUS_SQTR_2;
q2 = q3 = 0;
q4 = 0.92387956f;
q4 = HALF_SQRT_2_PlUS_SQRT_2;
return;
case ROTATION_YAW_180:
@ -160,9 +165,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_YAW_225:
q1 = -0.38268343f;
q1 = -HALF_SQRT_2_MINUS_SQTR_2;
q2 = q3 = 0;
q4 = 0.92387956f;
q4 = HALF_SQRT_2_PlUS_SQRT_2;
return;
case ROTATION_YAW_270:
@ -172,9 +177,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_YAW_315:
q1 = 0.92387956f;
q1 = HALF_SQRT_2_PlUS_SQRT_2;
q2 = q3 = 0;
q4 = -0.38268343f;
q4 = -HALF_SQRT_2_MINUS_SQTR_2;
return;
case ROTATION_ROLL_180:
@ -184,8 +189,8 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
case ROTATION_ROLL_180_YAW_45:
q1 = q4 = 0;
q2 = 0.92387956f;
q3 = 0.38268343f;
q2 = HALF_SQRT_2_PlUS_SQRT_2;
q3 = HALF_SQRT_2_MINUS_SQTR_2;
return;
case ROTATION_ROLL_180_YAW_90:
@ -196,8 +201,8 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
case ROTATION_ROLL_180_YAW_135:
q1 = q4 = 0;
q2 = 0.38268343f;
q3 = 0.92387956f;
q2 = HALF_SQRT_2_MINUS_SQTR_2;
q3 = HALF_SQRT_2_PlUS_SQRT_2;
return;
case ROTATION_PITCH_180:
@ -207,8 +212,8 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
case ROTATION_ROLL_180_YAW_225:
q1 = q4 = 0;
q2 = -0.38268343f;
q3 = 0.92387956f;
q2 = -HALF_SQRT_2_MINUS_SQTR_2;
q3 = HALF_SQRT_2_PlUS_SQRT_2;
return;
case ROTATION_ROLL_180_YAW_270:
@ -220,8 +225,8 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
case ROTATION_ROLL_180_YAW_315:
q1 = q4 = 0;
q2 = 0.92387956f;
q3 = -0.38268343f;
q2 = HALF_SQRT_2_PlUS_SQRT_2;
q3 = -HALF_SQRT_2_MINUS_SQTR_2;
return;
case ROTATION_ROLL_90:
@ -230,9 +235,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_90_YAW_45:
q1 = 0.65328151f;
q2 = 0.65328145f;
q3 = q4 = 0.27059802f;
q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q3 = q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
return;
case ROTATION_ROLL_90_YAW_90:
@ -240,9 +245,9 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_90_YAW_135:
q1 = q2 = 0.27059802f;
q3 = 0.65328145f;
q4 = 0.65328151f;
q1 = q2 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
q3 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
return;
case ROTATION_ROLL_270:
@ -252,10 +257,10 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_270_YAW_45:
q1 = 0.65328151f;
q2 = -0.65328145f;
q3 = -0.27059802f;
q4 = 0.27059802f;
q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q2 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
return;
case ROTATION_ROLL_270_YAW_90:
@ -264,10 +269,10 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_270_YAW_135:
q1 = 0.27059802f;
q2 = -0.27059802f;
q3 = -0.65328145f;
q4 = 0.65328151f;
q1 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
q2 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
q3 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
return;
case ROTATION_PITCH_90:
@ -334,40 +339,40 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_90_PITCH_68_YAW_293:
q1 = 0.26774535f;
q2 = 0.70698798f;
q3 = 0.01295743f;
q4 = -0.65445596f;
q1 = 0.26774500501681575137524760066299;
q2 = 0.70698804688952421315661922562867;
q3 = 0.012957683254962659713527273197542;
q4 = -0.65445596665363614530264158020145;
return;
case ROTATION_PITCH_315:
q1 = 0.92387956f;
q1 = HALF_SQRT_2_PlUS_SQRT_2;
q2 = q4 = 0;
q3 = -0.38268343f;
q3 = -HALF_SQRT_2_MINUS_SQTR_2;
return;
case ROTATION_ROLL_90_PITCH_315:
q1 = 0.65328151f;
q2 = 0.65328145f;
q3 = -0.27059802f;
q4 = 0.27059802f;
q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
return;
case ROTATION_PITCH_7:
q1 = 0.99813479f;
q1 = 0.99813479842186692003735970502021;
q2 = q4 = 0;
q3 = 0.06104854f;
q3 = 0.061048539534856872956769535676358;
return;
case ROTATION_ROLL_45:
q1 = 0.9238795325112867;
q2 = 0.3826834323650898;
q1 = HALF_SQRT_2_PlUS_SQRT_2;
q2 = HALF_SQRT_2_MINUS_SQTR_2;
q3 = q4 = 0.0;
return;
case ROTATION_ROLL_315:
q1 = 0.9238795325112867;
q2 = -0.3826834323650898;
q1 = HALF_SQRT_2_PlUS_SQRT_2;
q2 = -HALF_SQRT_2_MINUS_SQTR_2;
q3 = q4 = 0.0;
return;

4
libraries/AP_Math/rotations.h
View File

@ -63,7 +63,7 @@ enum Rotation : uint8_t {
ROTATION_ROLL_270_PITCH_270 = 35,
ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
ROTATION_ROLL_90_YAW_270 = 37,
ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, // this is actually, roll 90, pitch 68.8, yaw 293.3
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_PITCH_7 = 41,
@ -81,7 +81,7 @@ enum Rotation : uint8_t {
// definitions used by quaterion and vector3f
#define HALF_SQRT_2 0.70710678118654757f
#define HALF_SQRT_2 0.70710678118654752440084436210485
/*
Here are the same values in a form suitable for a @Values attribute in

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

@ -10,7 +10,7 @@
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define SQRT_2 1.4142135623730951f
#define SQRT_2 1.4142135623730950488016887242097
TEST(VectorTest, Rotations)
{
@ -74,10 +74,10 @@ TEST(VectorTest, Rotations)
TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.4066309f, -1.5839677f, -0.5706992f);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.40663092252764576617352076937095, -1.5839677018260314156350432313047, -0.57069923113341980425161636958364);
TEST_ROTATION(ROTATION_PITCH_315, 0, 1, SQRT_2);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_315, 0, -1, SQRT_2);
TEST_ROTATION(ROTATION_PITCH_7, 1.1144155f, 1, 0.87067682f);
TEST_ROTATION(ROTATION_PITCH_7, 1.1144154950464695286171945554088, 1, 0.87067680823617454866081288855639);
TEST_ROTATION(ROTATION_ROLL_45, 1, 0, SQRT_2);
TEST_ROTATION(ROTATION_ROLL_315, 1, SQRT_2, 0);
EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";

10
libraries/AP_Math/vector3.cpp
View File

@ -216,9 +216,9 @@ void Vector3<T>::rotate(enum Rotation rotation)
T tmpx = x;
T tmpy = y;
T tmpz = z;
x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz;
y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz;
z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz;
x = 0.14303897231223747232853327204793 * tmpx + 0.36877648650320382639478111741482 * tmpy + -0.91844638134308709265241077446262 * tmpz;
y = -0.33213277779664740485543461545603 * tmpx + -0.85628942146641884303193137384369 * tmpy + -0.39554550256296522325882847326284 * tmpz;
z = -0.93232380121551217122544130688766 * tmpx + 0.36162457008209242248497616856184 * tmpy + 0.00000000000000002214311861220361 * tmpz;
return;
}
case ROTATION_PITCH_315: {
@ -235,8 +235,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
return;
}
case ROTATION_PITCH_7: {
const T sin_pitch = 0.12186934340514748f; // sinF(pitch);
const T cos_pitch = 0.992546151641322f; // cosF(pitch);
const T sin_pitch = 0.1218693434051474899781908334262; // sinF(pitch);
const T cos_pitch = 0.99254615164132198312785249072476; // cosF(pitch);
T tmpx = x;
T tmpz = z;
x = cos_pitch * tmpx + sin_pitch * tmpz;