forked from Archive/PX4-Autopilot
commander: mag calibration auto rotation skip all duplicates
- add simple rotation test with corresponding blacklist
This commit is contained in:
parent
1dec1e6262
commit
08cfea0b6f
|
@ -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<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";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue