diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index bdf4b6c920..7a04dcbeaa 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -189,6 +189,7 @@ void QuaternionT::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::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::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; diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 519c685a9d..6a232d32c0 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -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, diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 55c3cfe6f5..cee78b9801 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -75,7 +75,8 @@ void Vector3::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::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::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;