diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 2327bd9a5187e120af15ecff9d0a12f38d7db60f..9473c815bd9946cd68c0a62a327cc993b8b7c53d 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -147,7 +147,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 226b9d1419d47dcb02f1a71c07805ca74b8a8c19..404f31dc5c652b36f63b766527c7eeae53254cf8 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 8ec1634b0d1ed99671ee88dafe4ba8e5fe057b61..ae5399df326d07093ed92b541e00f0a11e1ec61d 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -221,13 +221,13 @@ void Vector3<T>::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; } }