diff --git a/libraries/AP_Math/examples/rotations/rotations.cpp b/libraries/AP_Math/examples/rotations/rotations.cpp index f342f2eb7d..4b60fecfb2 100644 --- a/libraries/AP_Math/examples/rotations/rotations.cpp +++ b/libraries/AP_Math/examples/rotations/rotations.cpp @@ -220,6 +220,7 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90); test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); test_euler(ROTATION_ROLL_90_PITCH_68_YAW_293,90,68.8,293.3); + test_euler(ROTATION_PITCH_7, 0, 7, 0); } static bool have_rotation(const Matrix3f &m) diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index b381ca5691..3fd3a36c46 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -66,6 +66,7 @@ enum Rotation : uint8_t { ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, ROTATION_PITCH_315 = 39, ROTATION_ROLL_90_PITCH_315 = 40, + ROTATION_PITCH_7 = 41, /////////////////////////////////////////////////////////////////////// // Do not add more rotations without checking that there is not a conflict // with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 02ad46e12d..c868820a8d 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -244,6 +244,15 @@ void Vector3::rotate(enum Rotation rotation) x = tmp; return; } + case ROTATION_PITCH_7: { + const float sin_pitch = 0.12186934340514748f; // sinf(pitch); + const float cos_pitch = 0.992546151641322f; // cosf(pitch); + float tmpx = x; + float tmpz = z; + x = cos_pitch * tmpx + sin_pitch * tmpz; + z = -sin_pitch * tmpx + cos_pitch * tmpz; + return; + } case ROTATION_CUSTOM: // no-op; caller should perform custom rotations via matrix multiplication return; }