diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 29f06dd995..070a071c36 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -295,6 +295,50 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135); test_euler(ROTATION_PITCH_90, 0, 90, 0); test_euler(ROTATION_PITCH_270, 0, 270, 0); + test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90); + test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270); + test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0); + test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0); + test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0); + test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0); + test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0); + test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0); + test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0); + 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); +} + +static bool have_rotation(const Matrix3f &m) +{ + Matrix3f mt = m.transposed(); + for (enum Rotation r=ROTATION_NONE; + rprintln("testing for missing rotations"); + uint16_t roll, pitch, yaw; + for (yaw=0; yaw<360; yaw += 90) + for (pitch=0; pitch<360; pitch += 90) + for (roll=0; roll<360; roll += 90) { + Matrix3f m; + m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw)); + if (!have_rotation(m)) { + hal.console->printf("Missing rotation (%u, %u, %u)\n", roll, pitch, yaw); + } + } } /* @@ -310,6 +354,7 @@ void setup(void) #endif test_rotation_accuracy(); test_eulers(); + missing_rotations(); hal.console->println("rotation unit tests done\n"); } diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 54afa10c98..152e8e68c2 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -52,11 +52,23 @@ enum Rotation { 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_ROLL_90_PITCH_90 = 28, + ROTATION_ROLL_180_PITCH_90 = 29, + ROTATION_ROLL_270_PITCH_90 = 30, + ROTATION_ROLL_90_PITCH_180 = 31, + ROTATION_ROLL_270_PITCH_180 = 32, + ROTATION_ROLL_90_PITCH_270 = 33, + ROTATION_ROLL_180_PITCH_270 = 34, + ROTATION_ROLL_270_PITCH_270 = 35, + ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, + ROTATION_ROLL_90_YAW_270 = 37, ROTATION_MAX }; /* Here are the same values in a form sutable for a @Values attribute in auto documentation: -@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270 +@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270 */ diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 6ea5922e3b..04382dd50b 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -160,6 +160,67 @@ 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; + return; + } + case ROTATION_ROLL_180_PITCH_90: { + y = -y; z = -z; + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_ROLL_270_PITCH_90: { + tmp = z; z = -y; y = tmp; + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_ROLL_90_PITCH_180: { + tmp = z; z = y; y = -tmp; + x = -x; z = -z; + return; + } + case ROTATION_ROLL_270_PITCH_180: { + tmp = z; z = -y; y = tmp; + x = -x; z = -z; + return; + } + case ROTATION_ROLL_90_PITCH_270: { + tmp = z; z = y; y = -tmp; + tmp = z; z = x; x = -tmp; + return; + } + case ROTATION_ROLL_180_PITCH_270: { + y = -y; z = -z; + tmp = z; z = x; x = -tmp; + return; + } + case ROTATION_ROLL_270_PITCH_270: { + tmp = z; z = -y; y = tmp; + tmp = z; z = x; x = -tmp; + return; + } + case ROTATION_ROLL_90_PITCH_180_YAW_90: { + tmp = z; z = y; y = -tmp; + x = -x; z = -z; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_90_YAW_270: { + tmp = z; z = y; y = -tmp; + tmp = x; x = y; y = -tmp; + return; + } } }