zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Math: move to 32 digit rotation constants

gps-1.3.1
Iampete1 4 years ago committed by Andrew Tridgell
parent
78e025af39
commit
22100dd37c
4 changed files with 61 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 97
      libraries/AP_Math/quaternion.cpp
  2. 4
      libraries/AP_Math/rotations.h
  3. 6
      libraries/AP_Math/tests/test_math.cpp
  4. 10
      libraries/AP_Math/vector3.cpp

97
libraries/AP_Math/quaternion.cpp
Unescape View File

@ -21,6 +21,11 @@ @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -63,7 +63,7 @@ enum Rotation : uint8_t { @@ -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 { @@ -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
Unescape View File

@ -10,7 +10,7 @@ @@ -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) @@ -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
Unescape View File

@ -216,9 +216,9 @@ void Vector3<T>::rotate(enum Rotation rotation) @@ -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) @@ -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;

Write Preview
Loading…
Cancel
Save