commander: mag calibration auto rotation skip all duplicates

- add simple rotation test with corresponding blacklist
This commit is contained in:
Daniel Agar 2020-08-25 15:17:49 -04:00
parent 1dec1e6262
commit 08cfea0b6f
2 changed files with 72 additions and 1 deletions

View File

@ -63,3 +63,62 @@ TEST(Rotations, matrix_vs_3f)
EXPECT_NEAR(transformed_mat(2), transformed_3f(2), 10e-6); 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<Rotation>(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<Rotation>(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";
}
}
}
}

View File

@ -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 case ROTATION_ROLL_90_PITCH_68_YAW_293: // skip
// FALLTHROUGH // 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 // FALLTHROUGH
case ROTATION_PITCH_9_YAW_180: // skip, too close to ROTATION_YAW_180 case ROTATION_PITCH_9_YAW_180: // skip, too close to ROTATION_YAW_180