Compass_cal: implement the concept of auto and mandatory user reboot after cal

libraries/AP_Arming/AP_Arming.cpp

@@ -242,6 +242,22 @@ bool AP_Arming::compass_checks(bool report)
             return false;
         }
 
+        //check if compass is calibrating
+        if(_compass.is_calibrating()) {
+            if (report) {
+                gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Compass calibration running"));
+            }
+            return false;
+        }
+
+        //check if compass has calibrated and requires reboot
+        if(_compass.compass_cal_requires_reboot()) {
+            if (report) {
+                gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass calibrated requires reboot"));
+            }
+            return false;
+        }
+
         // check for unreasonable compass offsets
         Vector3f offsets = _compass.get_offsets();
         if (offsets.length() > 600) {

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -24,12 +24,20 @@ Compass::compass_cal_update()
             AP_Notify::flags.compass_cal_running = 1;
         }
     }
+    if (is_calibrating()) {
+        _cal_has_run = true;
+        return;
+    } 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)
+Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bool autoreboot)
 {
     if (healthy(i)) {
+        memset(_reports_sent,0,sizeof(_reports_sent));
         if (!is_calibrating() && delay > 0.5f) {
             AP_Notify::events.initiated_compass_cal = 1;
         }
@@ -39,6 +47,7 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
             _calibrator[i].set_tolerance(16);
         }
         _calibrator[i].start(retry, autosave, delay);
+        _compass_cal_autoreboot = autoreboot;
         return true;
     } else {
         return false;
@@ -46,11 +55,11 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
 }
 
 bool
-Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay)
+Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
 {
     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
         if ((1<<i) & mask) {
-            if (!start_calibration(i,retry,autosave,delay)) {
+            if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
                 cancel_calibration_mask(mask);
                 return false;
             }
@@ -60,11 +69,11 @@ Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float d
 }
 
 bool
-Compass::start_calibration_all(bool retry, bool autosave, float delay)
+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)) {
+            if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
                 cancel_calibration_all();
                 return false;
             }
@@ -104,6 +113,7 @@ Compass::accept_calibration(uint8_t i)
     uint8_t cal_status = cal.get_status();
 
     if (cal_status == COMPASS_CAL_SUCCESS) {
+        _cal_complete_requires_reboot = true;
         Vector3f ofs, diag, offdiag;
         cal.get_calibration(ofs, diag, offdiag);
         cal.clear();
@@ -194,8 +204,8 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
         uint8_t& compass_id = i;
         uint8_t cal_status = cal.get_status();
 
-        if (cal_status == COMPASS_CAL_SUCCESS ||
+        if ((cal_status == COMPASS_CAL_SUCCESS ||
-            cal_status == COMPASS_CAL_FAILED) {
+            cal_status == COMPASS_CAL_FAILED) && ((_reports_sent[i] < MAX_CAL_REPORTS) || CONTINUOUS_REPORTS)) {
             float fitness = cal.get_fitness();
             Vector3f ofs, diag, offdiag;
             cal.get_calibration(ofs, diag, offdiag);
@@ -210,6 +220,7 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
                 diag.x, diag.y, diag.z,
                 offdiag.x, offdiag.y, offdiag.z
             );
+            _reports_sent[i]++;
         }
 
         if (cal_status == COMPASS_CAL_SUCCESS && cal.get_autosave()) {

libraries/AP_Compass/Compass.cpp

@@ -378,13 +378,16 @@ Compass::Compass(void) :
     _board_orientation(ROTATION_NONE),
     _null_init_done(false),
     _thr_or_curr(0.0f),
-    _hil_mode(false)
+    _hil_mode(false),
+    _cal_complete_requires_reboot(false),
+    _cal_has_run(false)
 {
     AP_Param::setup_object_defaults(this, var_info);
     for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
         _backends[i] = NULL;
         _state[i].last_update_usec = 0;
-    }    
+        _reports_sent[i] = 0;
+    }
 
 #if COMPASS_MAX_INSTANCES > 1
     // default device ids to zero.  init() method will overwrite with the actual device ids

libraries/AP_Compass/Compass.h

@@ -46,6 +46,10 @@
 #define COMPASS_MAX_BACKEND   1   
 #endif
 
+//MAXIMUM COMPASS REPORTS
+#define MAX_CAL_REPORTS 10
+#define CONTINUOUS_REPORTS 0
+
 class Compass
 {
 friend class AP_Compass_Backend;
@@ -131,9 +135,9 @@ 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 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 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 start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
 
     void cancel_calibration(uint8_t i);
     void cancel_calibration_all();
@@ -143,6 +147,8 @@ public:
     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;
 
@@ -299,6 +305,14 @@ private:
     void _add_backend(AP_Compass_Backend *backend);
     void _detect_backends(void);
 
+    //keep track of number of calibration reports sent
+    uint8_t _reports_sent[COMPASS_MAX_INSTANCES];
+    
+    //autoreboot after compass calibration
+    bool _compass_cal_autoreboot;
+    bool _cal_complete_requires_reboot;
+    bool _cal_has_run;
+
     // backend objects
     AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
     uint8_t     _backend_count;