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:
parent
dc0c9dee0c
commit
32c5e6bced
libraries/AP_Compass
@ -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);
|
||||
|
@ -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;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user