diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 3d9546d1aa..ef05f11379 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -148,7 +148,7 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0); 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_YAW_293_PITCH_68_ROLL_180,180,68.8,293.3); + test_euler(ROTATION_YAW_293_PITCH_68_ROLL_90,90,68.8,293.3); } static bool have_rotation(const Matrix3f &m) diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 226b9d1419..404f31dc5c 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -64,7 +64,7 @@ enum Rotation { ROTATION_ROLL_270_PITCH_270 = 35, ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, ROTATION_ROLL_90_YAW_270 = 37, - ROTATION_YAW_293_PITCH_68_ROLL_180 = 38, + ROTATION_YAW_293_PITCH_68_ROLL_90 = 38, ROTATION_MAX }; /* diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 8ec1634b0d..ae5399df32 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -221,13 +221,13 @@ void Vector3::rotate(enum Rotation rotation) tmp = x; x = y; y = -tmp; return; } - case ROTATION_YAW_293_PITCH_68_ROLL_180: { + case ROTATION_YAW_293_PITCH_68_ROLL_90: { float tmpx = x; float tmpy = y; float tmpz = z; - x = 0.1430389f * tmpx -0.9184465f * tmpy -0.3687762f * tmpz; - y = -0.3321327f * tmpx -0.3955452f * tmpy +0.8562895f * tmpz; - z = -0.9323238f * tmpx -0.00000003f * tmpy -0.3616245f * tmpz; + 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; return; } }