AP_Compass: improved error reporting, check all compasses

this uses extensions to the MAG_CAL_REPORT message to convey failures
of orientation checking.

It also checks all compasses, external or internal. It only tries to
fix the orientation if it is external

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -61,11 +61,11 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
         // lot noisier
         _calibrator[i].set_tolerance(_calibration_threshold*2);
     }
-    if (_state[i].external && _rotate_auto) {
-        _calibrator[i].set_orientation((enum Rotation)_state[i].orientation.get(), _state[i].external);
+    if (_rotate_auto) {
+        _calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external);
     }
     _cal_saved[i] = false;
-    _calibrator[i].start(retry, delay, get_offsets_max());
+    _calibrator[i].start(retry, delay, get_offsets_max(), i);
 
     // disable compass learning both for calibration and after completion
     _learn.set_and_save(0);
@@ -222,8 +222,9 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
         }
 
         uint8_t cal_status = _calibrator[compass_id].get_status();
-        if ((cal_status == COMPASS_CAL_SUCCESS ||
-            cal_status == COMPASS_CAL_FAILED)) {
+        if (cal_status == COMPASS_CAL_SUCCESS ||
+            cal_status == COMPASS_CAL_FAILED ||
+            cal_status == COMPASS_CAL_BAD_ORIENTATION) {
             float fitness = _calibrator[compass_id].get_fitness();
             Vector3f ofs, diag, offdiag;
             _calibrator[compass_id].get_calibration(ofs, diag, offdiag);
@@ -236,7 +237,10 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
                 fitness,
                 ofs.x, ofs.y, ofs.z,
                 diag.x, diag.y, diag.z,
-                offdiag.x, offdiag.y, offdiag.z
+                offdiag.x, offdiag.y, offdiag.z,
+                _calibrator[compass_id].get_orientation_confidence(),
+                _calibrator[compass_id].get_original_orientation(),
+                _calibrator[compass_id].get_orientation()
             );
         }
     }
@@ -250,6 +254,7 @@ Compass::is_calibrating() const
             case COMPASS_CAL_NOT_STARTED:
             case COMPASS_CAL_SUCCESS:
             case COMPASS_CAL_FAILED:
+            case COMPASS_CAL_BAD_ORIENTATION:
                 break;
             default:
                 return true;

libraries/AP_Compass/CompassCalibrator.cpp

@@ -80,7 +80,8 @@ void CompassCalibrator::clear() {
     set_status(COMPASS_CAL_NOT_STARTED);
 }
 
-void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
+void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
+{
     if(running()) {
         return;
     }
@@ -89,6 +90,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
     _retry = retry;
     _delay_start_sec = delay;
     _start_time_ms = AP_HAL::millis();
+    _compass_idx = compass_idx;
     set_status(COMPASS_CAL_WAITING_TO_START);
 }
 
@@ -116,6 +118,7 @@ float CompassCalibrator::get_completion_percent() const {
         case COMPASS_CAL_SUCCESS:
             return 100.0f;
         case COMPASS_CAL_FAILED:
+        case COMPASS_CAL_BAD_ORIENTATION:
         default:
             return 0.0f;
     };
@@ -317,6 +320,13 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
             return true;
 
         case COMPASS_CAL_FAILED:
+            if (status == COMPASS_CAL_BAD_ORIENTATION) {
+                // don't overwrite bad orientation status
+                return false;
+            }
+            FALLTHROUGH;
+            
+        case COMPASS_CAL_BAD_ORIENTATION:
             if(_status == COMPASS_CAL_NOT_STARTED) {
                 return false;
             }
@@ -331,7 +341,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
                 _sample_buffer = nullptr;
             }
 
-            _status = COMPASS_CAL_FAILED;
+            _status = status;
             return true;
             
         default:
@@ -752,7 +762,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
 bool CompassCalibrator::calculate_orientation(void)
 {
     if (!_auto_orientation) {
-        // only calculate orientation for external compasses
+        // we are not checking orientation
         return true;
     }
 
@@ -795,25 +805,29 @@ bool CompassCalibrator::calculate_orientation(void)
             }
         }
     }
+
+    _orientation_confidence = second_best/bestv;
+    
     bool pass;
     if (besti == _orientation) {
         // if the orientation matched then allow for a low threshold
         pass = true;
     } else {
-        pass = (second_best / bestv) > 4;
+        pass = _orientation_confidence > 4;
     }
     if (!pass) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad orientation: %u %.1f\n", besti, second_best/bestv);
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
     } else if (besti == _orientation) {
         // no orientation change
-        gcs().send_text(MAV_SEVERITY_INFO, "Good orientation: %u %.1f\n", besti, second_best/bestv);
+        gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
+    } else if (!_is_external) {
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
     } else {
-        gcs().send_text(MAV_SEVERITY_INFO, "New orientation: %u was %u %.1f\n", besti, _orientation, second_best/bestv);
+        gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence);
     }
 
     if (!pass) {
-        // give an indication of orientation failure by showing a very high fitness
-        _fitness += 1000;
+        set_status(COMPASS_CAL_BAD_ORIENTATION);
         return false;
     }
 
@@ -822,6 +836,12 @@ bool CompassCalibrator::calculate_orientation(void)
         return true;
     }
 
+    if (!_is_external) {
+        // we can't change the orientation on internal compasses
+        set_status(COMPASS_CAL_BAD_ORIENTATION);
+        return false;
+    }
+    
     // correct the offsets for the new orientation
     Vector3f rot_offsets = _params.offset;
     rot_offsets.rotate_inverse(_orientation);

libraries/AP_Compass/CompassCalibrator.h

@@ -15,7 +15,8 @@ enum compass_cal_status_t {
     COMPASS_CAL_RUNNING_STEP_ONE=2,
     COMPASS_CAL_RUNNING_STEP_TWO=3,
     COMPASS_CAL_SUCCESS=4,
-    COMPASS_CAL_FAILED=5
+    COMPASS_CAL_FAILED=5,
+    COMPASS_CAL_BAD_ORIENTATION=6,
 };
 
 class CompassCalibrator {
@@ -24,7 +25,7 @@ public:
 
     CompassCalibrator();
 
-    void start(bool retry, float delay, uint16_t offset_max);
+    void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
     void clear();
 
     void update(bool &failure);
@@ -37,6 +38,7 @@ public:
     void set_orientation(enum Rotation orientation, bool is_external) {
         _auto_orientation = true;
         _orientation = orientation;
+        _orig_orientation = orientation;
         _is_external = is_external;
     }
     
@@ -44,11 +46,13 @@ public:
 
     void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
     enum Rotation get_orientation(void) { return _orientation; }
+    enum Rotation get_original_orientation(void) { return _orig_orientation; }
 
     float get_completion_percent() const;
     completion_mask_t& get_completion_mask();
     enum compass_cal_status_t get_status() const { return _status; }
     float get_fitness() const { return sqrtf(_fitness); }
+    float get_orientation_confidence() const { return _orientation_confidence; }
     uint8_t get_attempt() const { return _attempt; }
 
 private:
@@ -91,8 +95,10 @@ private:
     };
 
     enum Rotation _orientation;
+    enum Rotation _orig_orientation;
     bool _is_external;
     bool _auto_orientation;
+    uint8_t _compass_idx;
 
     enum compass_cal_status_t _status;
 
@@ -119,6 +125,7 @@ private:
     float _ellipsoid_lambda;
     uint16_t _samples_collected;
     uint16_t _samples_thinned;
+    float _orientation_confidence;
 
     bool set_status(compass_cal_status_t status);