Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: make duplicate rotations clear

This commit is contained in:
Iampete1 2021-07-23 20:47:21 +01:00 committed by Andrew Tridgell
parent 080f6c295c
commit dbd95f8163
3 changed files with 8 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Math/quaternion.cpp
View File

@ -189,6 +189,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_180_YAW_90:
case ROTATION_PITCH_180_YAW_270:
q1 = q4 = 0;
q2 = q3 = HALF_SQRT_2;
return;
@ -211,6 +212,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_180_YAW_270:
case ROTATION_PITCH_180_YAW_90:
q1 = q4 = 0;
q2 = -HALF_SQRT_2;
q3 = HALF_SQRT_2;
@ -279,17 +281,6 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
q3 = -HALF_SQRT_2;
return;
case ROTATION_PITCH_180_YAW_90:
q1 = q4 = 0;
q2 = -HALF_SQRT_2;
q3 = HALF_SQRT_2;
return;
case ROTATION_PITCH_180_YAW_270:
q1 = q4 = 0;
q2 = q3 = HALF_SQRT_2;
return;
case ROTATION_ROLL_90_PITCH_90:
q1 = q2 = q3 = -0.5f;
q4 = 0.5f;

4
libraries/AP_Math/rotations.h
View File

@ -51,8 +51,8 @@ enum Rotation : uint8_t {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_PITCH_180_YAW_90 = 26,
ROTATION_PITCH_180_YAW_270 = 27,
ROTATION_PITCH_180_YAW_90 = 26, // same as ROTATION_ROLL_180_YAW_270
ROTATION_PITCH_180_YAW_270 = 27, // same as ROTATION_ROLL_180_YAW_90
ROTATION_ROLL_90_PITCH_90 = 28,
ROTATION_ROLL_180_PITCH_90 = 29,
ROTATION_ROLL_270_PITCH_90 = 30,

16
libraries/AP_Math/vector3.cpp
View File

@ -75,7 +75,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_90: {
case ROTATION_ROLL_180_YAW_90:
case ROTATION_PITCH_180_YAW_270: {
tmp = x; x = y; y = tmp; z = -z;
return;
}
@ -95,7 +96,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_270: {
case ROTATION_ROLL_180_YAW_270:
case ROTATION_PITCH_180_YAW_90: {
tmp = x; x = -y; y = -tmp; z = -z;
return;
}
@ -159,16 +161,6 @@ void Vector3<T>::rotate(enum Rotation rotation)
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_PITCH_180_YAW_90: {
z = -z;
tmp = -x; x = -y; y = tmp;
return;
}
case ROTATION_PITCH_180_YAW_270: {
x = -x; z = -z;
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_ROLL_90_PITCH_90: {
tmp = z; z = y; y = -tmp;
tmp = z; z = -x; x = tmp;