diff --git a/src/lib/conversion/RotationTest.cpp b/src/lib/conversion/RotationTest.cpp index ecff2a40b4..2d125cde89 100644 --- a/src/lib/conversion/RotationTest.cpp +++ b/src/lib/conversion/RotationTest.cpp @@ -63,3 +63,62 @@ TEST(Rotations, matrix_vs_3f) EXPECT_NEAR(transformed_mat(2), transformed_3f(2), 10e-6); } } + +TEST(Rotations, duplicates) +{ + // find all identical rotations to skip (needs to be kept in sync with mag calibration auto rotation) + for (size_t i = 0; i < (size_t)Rotation::ROTATION_MAX; i++) { + + // GIVEN: an initial vector and a rotation + const matrix::Vector3f original = {1.f, 2.f, 3.f}; + const enum Rotation rotation_1 = static_cast(i); + + // WHEN: we transform the vector using the rotate_3f function and the rotation matrix + matrix::Vector3f transformed_1 = original; + rotate_3f(rotation_1, transformed_1(0), transformed_1(1), transformed_1(2)); + + for (size_t j = 0; j < (size_t)Rotation::ROTATION_MAX; j++) { + + const enum Rotation rotation_2 = static_cast(j); + + matrix::Vector3f transformed_2 = original; + rotate_3f(rotation_2, transformed_2(0), transformed_2(1), transformed_2(2)); + + if (i != j) { + // ROTATION_PITCH_180_YAW_90 (26) = ROTATION_ROLL_180_YAW_270 (14) + if (i == ROTATION_ROLL_180_YAW_270 && j == ROTATION_PITCH_180_YAW_90) { continue; } + + if (j == ROTATION_ROLL_180_YAW_270 && i == ROTATION_PITCH_180_YAW_90) { continue; } + + + // ROTATION_ROLL_180_YAW_90 (10) = ROTATION_PITCH_180_YAW_270 (27) + if (i == ROTATION_ROLL_180_YAW_90 && j == ROTATION_PITCH_180_YAW_270) { continue; } + + if (j == ROTATION_ROLL_180_YAW_90 && i == ROTATION_PITCH_180_YAW_270) { continue; } + + + // ROTATION_ROLL_180_PITCH_90 (29) = ROTATION_PITCH_90_YAW_180 (43) + if (i == ROTATION_ROLL_180_PITCH_90 && j == ROTATION_PITCH_90_YAW_180) { continue; } + + if (j == ROTATION_ROLL_180_PITCH_90 && i == ROTATION_PITCH_90_YAW_180) { continue; } + + + // ROTATION_ROLL_90_PITCH_180 (31) = ROTATION_ROLL_270_YAW_180 (41) + if (i == ROTATION_ROLL_90_PITCH_180 && j == ROTATION_ROLL_270_YAW_180) { continue; } + + if (j == ROTATION_ROLL_90_PITCH_180 && i == ROTATION_ROLL_270_YAW_180) { continue; } + + + // ROTATION_ROLL_90_PITCH_180_YAW_90 (36) = ROTATION_ROLL_270_YAW_270 (42) + if (i == ROTATION_ROLL_90_PITCH_180_YAW_90 && j == ROTATION_ROLL_270_YAW_270) { continue; } + + if (j == ROTATION_ROLL_90_PITCH_180_YAW_90 && i == ROTATION_ROLL_270_YAW_270) { continue; } + + + + // otherwise all rotations should be different + ASSERT_GT((transformed_1 - transformed_2).norm(), 0) << "Rotation " << i << " and " << j << " equal"; + } + } + } +} diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 3665957c16..6ee7e78fed 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -724,7 +724,19 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma case ROTATION_ROLL_90_PITCH_68_YAW_293: // skip // FALLTHROUGH - case ROTATION_ROLL_270_YAW_270: // skip, same as ROTATION_ROLL_90_PITCH_180_YAW_90 (36) + case ROTATION_PITCH_180_YAW_90: // skip 26, same as 14 ROTATION_ROLL_180_YAW_270 + + // FALLTHROUGH + case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90 + + // FALLTHROUGH + case ROTATION_ROLL_270_YAW_180: // skip 41, same as 31 ROTATION_ROLL_90_PITCH_180 + + // FALLTHROUGH + case ROTATION_ROLL_270_YAW_270: // skip 42, same as 36 ROTATION_ROLL_90_PITCH_180_YAW_90 + + // FALLTHROUGH + case ROTATION_PITCH_90_YAW_180: // skip 43, same as 29 ROTATION_ROLL_180_PITCH_90 // FALLTHROUGH case ROTATION_PITCH_9_YAW_180: // skip, too close to ROTATION_YAW_180