Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 00:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fixed handling of duplication rotations

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -768,6 +768,23 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
return efield; 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 calculate compass orientation using the attitude estimate associated
with each sample, and fix orientation on external compasses if 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; const float variance_threshold = 2.0;
float second_best = besti==ROTATION_NONE?variance[1]:variance[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)) { 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) { if (variance[r] < second_best) {
second_best = variance[r]; second_best = variance[r];
besti2 = r;
} }
} }
} }
@ -833,7 +852,8 @@ bool CompassCalibrator::calculate_orientation(void)
pass = _orientation_confidence > variance_threshold; pass = _orientation_confidence > variance_threshold;
} }
if (!pass) { 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) { } else if (besti == _orientation) {
// no orientation change // no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, _orientation_confidence); gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);

1
libraries/AP_Compass/CompassCalibrator.h
View File

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