From 4d8053afc91a0f36973a3352b115b83689acf66c Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 24 Oct 2016 22:16:11 -0700
Subject: [PATCH] Compass: Rework compass calibrator

Summary of significant changes:
  -Autsave doesn't depend on STREAM_EXTRA3
  -Don't risk only saving one compass on copter if CAL_ALWAYS_REBOOT is set
  -Only calibrate compasses that are both health and marked for use (there was a inconsistency in handling the mask)
  -Fix incorrect failure reporting on DO_ACCEPT_MAG_CAL with a mask of 0 if a channel was specifically not started
  -Fix not starting the buzzer if the delay is set to 0 seconds
  -Always send MAG_CAL_REPORT until its acknowledged
  -Correct the field in MAG_CAL_REPORT for autosave to indicate if the compass had actually been saved, rather then being scheduled to be saved
  -Remmove unused public interfaces
---
 libraries/AP_Compass/AP_Compass.cpp           |   1 -
 libraries/AP_Compass/AP_Compass.h             |  31 ++--
 .../AP_Compass/AP_Compass_Calibration.cpp     | 154 +++++++++---------
 libraries/AP_Compass/CompassCalibrator.cpp    |   3 +-
 libraries/AP_Compass/CompassCalibrator.h      |   4 +-
 5 files changed, 90 insertions(+), 103 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 015fc06a1f..4d8db95e30 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -383,7 +383,6 @@ Compass::Compass(void) :
     for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
         _backends[i] = NULL;
         _state[i].last_update_usec = 0;
-        _reports_sent[i] = 0;
     }
 
     // default device ids to zero.  init() method will overwrite with the actual device ids
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 551aed5bf0..66d5af9bbe 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -49,9 +49,6 @@
 #define COMPASS_MAX_INSTANCES 3
 #define COMPASS_MAX_BACKEND   3
 
-//MAXIMUM COMPASS REPORTS
-#define MAX_CAL_REPORTS 10
-#define CONTINUOUS_REPORTS 0
 #define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(50.0f)
 #define AP_COMPASS_MAX_XY_ANG_DIFF radians(30.0f)
 #define AP_COMPASS_MAX_XY_LENGTH_DIFF 100.0f
@@ -126,21 +123,11 @@ public:
     // compass calibrator interface
     void compass_cal_update();
 
-    bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
-    bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
-    bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
+    void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
 
-    void cancel_calibration(uint8_t i);
     void cancel_calibration_all();
-    void cancel_calibration_mask(uint8_t mask);
-
-    bool accept_calibration(uint8_t i);
-    bool accept_calibration_all();
-    bool accept_calibration_mask(uint8_t mask);
 
     bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; }
-    bool auto_reboot() { return _compass_cal_autoreboot; }
-    uint8_t get_cal_mask() const;
     bool is_calibrating() const;
 
     /*
@@ -316,8 +303,20 @@ private:
     bool _add_backend(AP_Compass_Backend *backend, const char *name, bool external);
     void _detect_backends(void);
 
-    //keep track of number of calibration reports sent
-    uint8_t _reports_sent[COMPASS_MAX_INSTANCES];
+    // compass cal
+    bool _accept_calibration(uint8_t i);
+    bool _accept_calibration_mask(uint8_t mask);
+    void _cancel_calibration(uint8_t i);
+    void _cancel_calibration_mask(uint8_t mask);
+    uint8_t _get_cal_mask() const;
+    bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f);
+    bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
+    bool _auto_reboot() { return _compass_cal_autoreboot; }
+
+
+    //keep track of which calibrators have been saved
+    bool _cal_saved[COMPASS_MAX_INSTANCES];
+    bool _cal_autosave;
 
     //autoreboot after compass calibration
     bool _compass_cal_autoreboot;
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 98cb92f4c8..2d3487b7ee 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -25,6 +25,8 @@ Compass::compass_cal_update()
 
         if (_calibrator[i].running()) {
             running = true;
+        } else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == COMPASS_CAL_SUCCESS) {
+            _accept_calibration(i);
         }
     }
 
@@ -33,20 +35,22 @@ Compass::compass_cal_update()
     if (is_calibrating()) {
         _cal_has_run = true;
         return;
-    } else if (_cal_has_run && auto_reboot()) {
+    } else if (_cal_has_run && _auto_reboot()) {
         hal.scheduler->delay(1000);
         hal.scheduler->reboot(false);
     }
 }
 
 bool
-Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bool autoreboot)
+Compass::_start_calibration(uint8_t i, bool retry, float delay)
 {
     if (!healthy(i)) {
         return false;
     }
-    memset(_reports_sent,0,sizeof(_reports_sent));
-    if (!is_calibrating() && delay > 0.5f) {
+    if (!use_for_yaw(i)) {
+        return false;
+    }
+    if (!is_calibrating()) {
         AP_Notify::events.initiated_compass_cal = 1;
     }
     if (i == get_primary() && _state[i].external != 0) {
@@ -57,8 +61,8 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bo
         // lot noisier
         _calibrator[i].set_tolerance(_calibration_threshold*2);
     }
-    _calibrator[i].start(retry, autosave, delay);
-    _compass_cal_autoreboot = autoreboot;
+    _cal_saved[i] = false;
+    _calibrator[i].start(retry, delay);
 
     // disable compass learning both for calibration and after completion
     _learn.set_and_save(0);
@@ -67,26 +71,15 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bo
 }
 
 bool
-Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
+Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
 {
+    _cal_autosave = autosave;
+    _compass_cal_autoreboot = autoreboot;
+
     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
         if ((1<<i) & mask) {
-            if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
-                cancel_calibration_mask(mask);
-                return false;
-            }
-        }
-    }
-    return true;
-}
-
-bool
-Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
-{
-    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
-        if (healthy(i) && use_for_yaw(i)) {
-            if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
-                cancel_calibration_all();
+            if (!_start_calibration(i,retry,delay)) {
+                _cancel_calibration_mask(mask);
                 return false;
             }
         }
@@ -95,22 +88,36 @@ Compass::start_calibration_all(bool retry, bool autosave, float delay, bool auto
 }
 
 void
-Compass::cancel_calibration(uint8_t i)
+Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
+{
+    _cal_autosave = autosave;
+    _compass_cal_autoreboot = autoreboot;
+
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        // ignore any compasses that fail to start calibrating
+        // start all should only calibrate compasses that are being used
+        _start_calibration(i,retry,delay);
+    }
+}
+
+void
+Compass::_cancel_calibration(uint8_t i)
 {
     AP_Notify::events.initiated_compass_cal = 0;
 
     if (_calibrator[i].running() || _calibrator[i].get_status() == COMPASS_CAL_WAITING_TO_START) {
         AP_Notify::events.compass_cal_canceled = 1;
     }
+    _cal_saved[i] = false;
     _calibrator[i].clear();
 }
 
 void
-Compass::cancel_calibration_mask(uint8_t mask)
+Compass::_cancel_calibration_mask(uint8_t mask)
 {
     for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
         if((1<<i) & mask) {
-            cancel_calibration(i);
+            _cancel_calibration(i);
         }
     }
 }
@@ -118,20 +125,23 @@ Compass::cancel_calibration_mask(uint8_t mask)
 void
 Compass::cancel_calibration_all()
 {
-    cancel_calibration_mask(0xFF);
+    _cancel_calibration_mask(0xFF);
 }
 
 bool
-Compass::accept_calibration(uint8_t i)
+Compass::_accept_calibration(uint8_t i)
 {
     CompassCalibrator& cal = _calibrator[i];
     uint8_t cal_status = cal.get_status();
 
-    if (cal_status == COMPASS_CAL_SUCCESS) {
+    if (_cal_saved[i] || cal_status == COMPASS_CAL_NOT_STARTED) {
+        return true;
+    } else if (cal_status == COMPASS_CAL_SUCCESS) {
         _cal_complete_requires_reboot = true;
+        _cal_saved[i] = true;
+
         Vector3f ofs, diag, offdiag;
         cal.get_calibration(ofs, diag, offdiag);
-        cal.clear();
 
         set_and_save_offsets(i, ofs);
         set_and_save_diagonals(i,diag);
@@ -147,43 +157,32 @@ Compass::accept_calibration(uint8_t i)
 }
 
 bool
-Compass::accept_calibration_mask(uint8_t mask)
+Compass::_accept_calibration_mask(uint8_t mask)
 {
-    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
-        if ((1<<i) & mask) {
-            CompassCalibrator& cal = _calibrator[i];
-            uint8_t cal_status = cal.get_status();
-            if (cal_status != COMPASS_CAL_SUCCESS && cal_status != COMPASS_CAL_NOT_STARTED) {
-                // a compass failed or is still in progress
-                return false;
-            }
-        }
-    }
-
     bool success = true;
     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
         if ((1<<i) & mask) {
-            if (!accept_calibration(i)) {
+            if (!_accept_calibration(i)) {
                 success = false;
             }
+            _calibrator[i].clear();
         }
     }
 
     return success;
 }
 
-bool
-Compass::accept_calibration_all()
-{
-    return accept_calibration_mask(0xFF);
-}
-
 void
 Compass::send_mag_cal_progress(mavlink_channel_t chan)
 {
-    uint8_t cal_mask = get_cal_mask();
+    uint8_t cal_mask = _get_cal_mask();
 
     for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
+        // ensure we don't try to send with no space available
+        if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
+            return;
+        }
+
         auto& calibrator = _calibrator[compass_id];
         uint8_t cal_status = calibrator.get_status();
 
@@ -195,11 +194,6 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
             Vector3f direction(0.0f,0.0f,0.0f);
             uint8_t attempt = _calibrator[compass_id].get_attempt();
 
-            // ensure we don't try to send with no space available
-            if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
-                return;
-            }
-
             mavlink_msg_mag_cal_progress_send(
                 chan,
                 compass_id, cal_mask,
@@ -212,24 +206,22 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
 
 void Compass::send_mag_cal_report(mavlink_channel_t chan)
 {
-    uint8_t cal_mask = get_cal_mask();
+    uint8_t cal_mask = _get_cal_mask();
 
     for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
+        // ensure we don't try to send with no space available
+        if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
+            return;
+        }
 
         uint8_t cal_status = _calibrator[compass_id].get_status();
-
         if ((cal_status == COMPASS_CAL_SUCCESS ||
-            cal_status == COMPASS_CAL_FAILED) && ((_reports_sent[compass_id] < MAX_CAL_REPORTS) || CONTINUOUS_REPORTS)) {
+            cal_status == COMPASS_CAL_FAILED)) {
             float fitness = _calibrator[compass_id].get_fitness();
             Vector3f ofs, diag, offdiag;
             _calibrator[compass_id].get_calibration(ofs, diag, offdiag);
-            uint8_t autosaved = _calibrator[compass_id].get_autosave();
+            uint8_t autosaved = _cal_saved[compass_id];
 
-            // ensure we don't try to send with no space available
-            if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
-                return;
-            }
-            
             mavlink_msg_mag_cal_report_send(
                 chan,
                 compass_id, cal_mask,
@@ -239,11 +231,6 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
                 diag.x, diag.y, diag.z,
                 offdiag.x, offdiag.y, offdiag.z
             );
-            _reports_sent[compass_id]++;
-        }
-
-        if (cal_status == COMPASS_CAL_SUCCESS && _calibrator[compass_id].get_autosave()) {
-            accept_calibration(compass_id);
         }
     }
 }
@@ -251,11 +238,21 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
 bool
 Compass::is_calibrating() const
 {
-    return get_cal_mask();
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        switch(_calibrator[i].get_status()) {
+            case COMPASS_CAL_NOT_STARTED:
+            case COMPASS_CAL_SUCCESS:
+            case COMPASS_CAL_FAILED:
+                break;
+            default:
+                return true;
+        }
+    }
+    return false;
 }
 
 uint8_t
-Compass::get_cal_mask() const
+Compass::_get_cal_mask() const
 {
     uint8_t cal_mask = 0;
     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
@@ -294,11 +291,9 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
         bool autoreboot = !is_zero(packet.param5);
 
         if (mag_mask == 0) { // 0 means all
-            if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
-                result = MAV_RESULT_FAILED;
-            }
+            start_calibration_all(retry, autosave, delay, autoreboot);
         } else {
-            if (!start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
+            if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
                 result = MAV_RESULT_FAILED;
             }
         }
@@ -316,13 +311,10 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
         uint8_t mag_mask = packet.param1;
         
         if (mag_mask == 0) { // 0 means all
-            if(!accept_calibration_all()) {
-                result = MAV_RESULT_FAILED;
-            }
-            break;
+            mag_mask = 0xFF;
         }
         
-        if(!accept_calibration_mask(mag_mask)) {
+        if(!_accept_calibration_mask(mag_mask)) {
             result = MAV_RESULT_FAILED;
         }
         break;
@@ -342,7 +334,7 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
             break;
         }
         
-        cancel_calibration_mask(mag_mask);
+        _cancel_calibration_mask(mag_mask);
         break;
     }
     }
diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp
index 75f8ad860a..f83493de37 100644
--- a/libraries/AP_Compass/CompassCalibrator.cpp
+++ b/libraries/AP_Compass/CompassCalibrator.cpp
@@ -86,11 +86,10 @@ void CompassCalibrator::clear() {
     set_status(COMPASS_CAL_NOT_STARTED);
 }
 
-void CompassCalibrator::start(bool retry, bool autosave, float delay) {
+void CompassCalibrator::start(bool retry, float delay) {
     if(running()) {
         return;
     }
-    _autosave = autosave;
     _attempt = 1;
     _retry = retry;
     _delay_start_sec = delay;
diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h
index de6b6c4785..16c1ef14aa 100644
--- a/libraries/AP_Compass/CompassCalibrator.h
+++ b/libraries/AP_Compass/CompassCalibrator.h
@@ -22,7 +22,7 @@ public:
 
     CompassCalibrator();
 
-    void start(bool retry=false, bool autosave=false, float delay=0.0f);
+    void start(bool retry=false, float delay=0.0f);
     void clear();
 
     void update(bool &failure);
@@ -40,7 +40,6 @@ public:
     completion_mask_t& get_completion_mask();
     enum compass_cal_status_t get_status() const { return _status; }
     float get_fitness() const { return sqrtf(_fitness); }
-    bool get_autosave() const { return _autosave; }
     uint8_t get_attempt() const { return _attempt; }
 
 private:
@@ -80,7 +79,6 @@ private:
     // behavioral state
     float _delay_start_sec;
     uint32_t _start_time_ms;
-    bool _autosave;
     bool _retry;
     float _tolerance;
     uint8_t _attempt;