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

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<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, (double)_orientation_confidence);
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
+                        besti, besti2, (double)_orientation_confidence);
     } else if (besti == _orientation) {
         // no orientation change
         gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);

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;
 };