diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 8f92f02346..f33cb3998b 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -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 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); diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index dadf00d564..3f339d5143 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -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; };