5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00

AP_Compass: fixed handling of duplication rotations

we have some rotations that are duplicated, such as ROLL_180_YAW_90
and PITCH_180_YAW_270. This copes with those in the auto-orientation
code
This commit is contained in:
Andrew Tridgell 2018-09-19 13:48:17 +10:00
parent dc0c9dee0c
commit 32c5e6bced
2 changed files with 23 additions and 2 deletions

View File

@ -768,6 +768,23 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
return efield;
}
/*
return true if two rotations are equivalent
This copes with the fact that we have some duplicates, like ROLL_180_YAW_90 and PITCH_180_YAW_270
*/
bool CompassCalibrator::rotation_equal(enum Rotation r1, enum Rotation r2) const
{
if (r1 == r2) {
return true;
}
Vector3f v(1,2,3);
Vector3f v1 = v;
Vector3f v2 = v;
v1.rotate(r1);
v2.rotate(r2);
return (v1 - v2).length() < 0.001;
}
/*
calculate compass orientation using the attitude estimate associated
with each sample, and fix orientation on external compasses if
@ -815,10 +832,12 @@ bool CompassCalibrator::calculate_orientation(void)
const float variance_threshold = 2.0;
float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
enum Rotation besti2 = ROTATION_NONE;
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (r != besti) {
if (!rotation_equal(besti, r)) {
if (variance[r] < second_best) {
second_best = variance[r];
besti2 = r;
}
}
}
@ -833,7 +852,8 @@ bool CompassCalibrator::calculate_orientation(void)
pass = _orientation_confidence > variance_threshold;
}
if (!pass) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
besti, besti2, _orientation_confidence);
} else if (besti == _orientation) {
// no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);

View File

@ -171,4 +171,5 @@ private:
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
bool calculate_orientation();
bool rotation_equal(enum Rotation r1, enum Rotation r2) const;
};