AP_Compass: pull compass calibrator out into a separate thread

libraries/AP_Compass/AP_Compass.h

@@ -9,7 +9,6 @@
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
-#include "CompassCalibrator.h"
 #include "AP_Compass_Backend.h"
 #include "Compass_PerMotor.h"
 #include <AP_Common/TSIndex.h>
@@ -63,6 +62,8 @@
 
 #define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
 
+#include "CompassCalibrator.h"
+
 class CompassLearn;
 
 class Compass
@@ -167,8 +168,8 @@ public:
 
     void cancel_calibration_all();
 
-    bool compass_cal_requires_reboot() const { return _cal_complete_requires_reboot; }
-    bool is_calibrating() const;
+    bool compass_cal_requires_reboot() const { return _cal_requires_reboot; }
+    bool is_calibrating();
 
     // indicate which bit in LOG_BITMASK indicates we should log compass readings
     void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
@@ -367,11 +368,12 @@ private:
     void _detect_backends(void);
 
     // compass cal
+    void _update_calibration_trampoline();
     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;
+    uint8_t _get_cal_mask();
     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; }
@@ -393,7 +395,7 @@ private:
 
     //autoreboot after compass calibration
     bool _compass_cal_autoreboot;
-    bool _cal_complete_requires_reboot;
+    bool _cal_requires_reboot;
     bool _cal_has_run;
 
     // enum of drivers for COMPASS_TYPEMASK
@@ -545,7 +547,7 @@ private:
     AP_Int16 _options;
 
 #if COMPASS_CAL_ENABLED
-    RestrictIDTypeArray<CompassCalibrator, COMPASS_MAX_INSTANCES, Priority> _calibrator;
+    RestrictIDTypeArray<CompassCalibrator*, COMPASS_MAX_INSTANCES, Priority> _calibrator;
 #endif
 
 #if COMPASS_MOT_ENABLED
@@ -578,6 +580,8 @@ private:
     ///
     void try_set_initial_location();
     bool _initial_location_set;
+
+    bool _cal_thread_started;
 };
 
 namespace AP {

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -52,7 +52,10 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance
     // sensor rate. We want them to consume only the filtered fields
     state.last_update_ms = AP_HAL::millis();
 #if COMPASS_CAL_ENABLED
-    _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))].new_sample(mag);
+    auto& cal = _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))];
+    if (cal != nullptr) {
+        cal->new_sample(mag);
+    }
 #endif
 }
 

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -3,7 +3,6 @@
 #include <AP_GPS/AP_GPS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
-#include <AP_GPS/AP_GPS.h>
 
 #include "AP_Compass.h"
 
@@ -20,20 +19,16 @@ void Compass::cal_update()
     bool running = false;
 
     for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        bool failure;
-        _calibrator[i].update(failure);
-        if (failure) {
-            AP_Notify::events.compass_cal_failed = 1;
+        if (_calibrator[i] == nullptr) {
+            continue;
         }
-
-        if (_calibrator[i].check_for_timeout()) {
+        if (_calibrator[i]->failed()) {
             AP_Notify::events.compass_cal_failed = 1;
-            cancel_calibration_all();
         }
 
-        if (_calibrator[i].running()) {
+        if (_calibrator[i]->running()) {
             running = true;
-        } else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == CompassCalibrator::Status::SUCCESS) {
+        } else if (_cal_autosave && !_cal_saved[i] && _calibrator[i]->get_state().status == CompassCalibrator::Status::SUCCESS) {
             _accept_calibration(uint8_t(i));
         }
     }
@@ -62,6 +57,15 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
         gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
         return false;
     }
+    
+    if (_calibrator[prio] == nullptr) {
+        _calibrator[prio] = new CompassCalibrator();
+        if (_calibrator[prio] == nullptr) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal object not initialised");
+            return false;
+        }
+    }
+
     if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
         if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
             gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
@@ -71,22 +75,30 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
     if (!is_calibrating()) {
         AP_Notify::events.initiated_compass_cal = 1;
     }
+
+    if (_rotate_auto) {
+        enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
+        if (r != ROTATION_CUSTOM) {
+            _calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2);
+        }
+    }
+    _cal_saved[prio] = false;
     if (i == 0 && _get_state(prio).external != 0) {
-        _calibrator[prio].set_tolerance(_calibration_threshold);
+        _calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold);
     } else {
         // internal compasses or secondary compasses get twice the
         // threshold. This is because internal compasses tend to be a
         // lot noisier
-        _calibrator[prio].set_tolerance(_calibration_threshold*2);
+        _calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold*2);
     }
-    if (_rotate_auto) {
-        enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
-        if (r != ROTATION_CUSTOM) {
-            _calibrator[prio].set_orientation(r, _get_state(prio).external, _rotate_auto>=2);
+    if (!_cal_thread_started) {
+        _cal_requires_reboot = true;
+        if (!hal.scheduler->thread_create(FUNCTOR_BIND(this, &Compass::_update_calibration_trampoline, void), "compasscal", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0)) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "CompassCalibrator: Cannot start compass thread.");
+            return false;
         }
+        _cal_thread_started = true;
     }
-    _cal_saved[prio] = false;
-    _calibrator[prio].start(retry, delay, get_offsets_max(), i);
 
     // disable compass learning both for calibration and after completion
     _learn.set_and_save(0);
@@ -94,6 +106,18 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
     return true;
 }
 
+void Compass::_update_calibration_trampoline() {
+    while(true) {
+        for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
+            if (_calibrator[i] == nullptr) {
+                continue;
+            }
+            _calibrator[i]->update();
+        }
+        hal.scheduler->delay(1);
+    }
+}
+
 bool Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
 {
     _cal_autosave = autosave;
@@ -126,12 +150,14 @@ void Compass::_cancel_calibration(uint8_t i)
 {
     AP_Notify::events.initiated_compass_cal = 0;
     Priority prio = Priority(i);
-
-    if (_calibrator[prio].running() || _calibrator[prio].get_status() == CompassCalibrator::Status::WAITING_TO_START) {
+    if (_calibrator[prio] == nullptr) {
+        return;
+    }
+    if (_calibrator[prio]->running() || _calibrator[prio]->get_state().status == CompassCalibrator::Status::WAITING_TO_START) {
         AP_Notify::events.compass_cal_canceled = 1;
     }
     _cal_saved[prio] = false;
-    _calibrator[prio].stop();
+    _calibrator[prio]->stop();
 }
 
 void Compass::_cancel_calibration_mask(uint8_t mask)
@@ -152,18 +178,19 @@ bool Compass::_accept_calibration(uint8_t i)
 {
     Priority prio = Priority(i);
 
-    CompassCalibrator& cal = _calibrator[prio];
-    const CompassCalibrator::Status cal_status = cal.get_status();
+    CompassCalibrator* cal = _calibrator[prio];
+    if (cal == nullptr) {
+        return false;
+    }
+    const CompassCalibrator::Report cal_report = cal->get_report();
 
-    if (_cal_saved[prio] || cal_status == CompassCalibrator::Status::NOT_STARTED) {
+    if (_cal_saved[prio] || cal_report.status == CompassCalibrator::Status::NOT_STARTED) {
         return true;
-    } else if (cal_status == CompassCalibrator::Status::SUCCESS) {
-        _cal_complete_requires_reboot = true;
+    } else if (cal_report.status == CompassCalibrator::Status::SUCCESS) {
         _cal_saved[prio] = true;
 
-        Vector3f ofs, diag, offdiag;
-        float scale_factor;
-        cal.get_calibration(ofs, diag, offdiag, scale_factor);
+        Vector3f ofs(cal_report.ofs), diag(cal_report.diag), offdiag(cal_report.offdiag);
+        float scale_factor = cal_report.scale_factor;
 
         set_and_save_offsets(i, ofs);
         set_and_save_diagonals(i,diag);
@@ -171,7 +198,7 @@ bool Compass::_accept_calibration(uint8_t i)
         set_and_save_scale_factor(i,scale_factor);
 
         if (_get_state(prio).external && _rotate_auto >= 2) {
-            set_and_save_orientation(i, cal.get_orientation());
+            set_and_save_orientation(i, cal_report.orientation);
         }
 
         if (!is_calibrating()) {
@@ -187,11 +214,14 @@ bool Compass::_accept_calibration_mask(uint8_t mask)
 {
     bool success = true;
     for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
+        if (_calibrator[i] == nullptr) {
+            continue;
+        }
         if ((1<<uint8_t(i)) & mask) {
             if (!_accept_calibration(uint8_t(i))) {
                 success = false;
             }
-            _calibrator[i].stop();
+            _calibrator[i]->stop();
         }
     }
 
@@ -210,23 +240,21 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
             // hogging *all* the bandwidth
             return true;
         }
-
+        
         auto& calibrator = _calibrator[compass_id];
-        const CompassCalibrator::Status cal_status = calibrator.get_status();
-
-        if (cal_status == CompassCalibrator::Status::WAITING_TO_START  ||
-            cal_status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
-            cal_status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
-            uint8_t completion_pct = calibrator.get_completion_percent();
-            const CompassCalibrator::completion_mask_t& completion_mask = calibrator.get_completion_mask();
-            const Vector3f direction;
-            uint8_t attempt = _calibrator[compass_id].get_attempt();
+        if (calibrator == nullptr) {
+            continue;
+        }
+        const CompassCalibrator::State cal_state = calibrator->get_state();
 
+        if (cal_state.status == CompassCalibrator::Status::WAITING_TO_START  ||
+            cal_state.status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
+            cal_state.status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
             mavlink_msg_mag_cal_progress_send(
                 link.get_chan(),
                 uint8_t(compass_id), cal_mask,
-                (uint8_t)cal_status, attempt, completion_pct, completion_mask,
-                direction.x, direction.y, direction.z
+                (uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
+                0.0f, 0.0f, 0.0f
             );
         }
     }
@@ -246,39 +274,39 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
             // hogging *all* the bandwidth
             return true;
         }
-
-        const CompassCalibrator::Status cal_status = _calibrator[compass_id].get_status();
-        if (cal_status == CompassCalibrator::Status::SUCCESS ||
-            cal_status == CompassCalibrator::Status::FAILED ||
-            cal_status == CompassCalibrator::Status::BAD_ORIENTATION) {
-            float fitness = _calibrator[compass_id].get_fitness();
-            Vector3f ofs, diag, offdiag;
-            float scale_factor;
-            _calibrator[compass_id].get_calibration(ofs, diag, offdiag, scale_factor);
+        if (_calibrator[compass_id] == nullptr) {
+            continue;
+        }
+        const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report();
+        if (cal_report.status == CompassCalibrator::Status::SUCCESS ||
+            cal_report.status == CompassCalibrator::Status::FAILED ||
+            cal_report.status == CompassCalibrator::Status::BAD_ORIENTATION) {
             uint8_t autosaved = _cal_saved[compass_id];
-
             mavlink_msg_mag_cal_report_send(
                 link.get_chan(),
                 uint8_t(compass_id), cal_mask,
-                (uint8_t)cal_status, autosaved,
-                fitness,
-                ofs.x, ofs.y, ofs.z,
-                diag.x, diag.y, diag.z,
-                offdiag.x, offdiag.y, offdiag.z,
-                _calibrator[compass_id].get_orientation_confidence(),
-                _calibrator[compass_id].get_original_orientation(),
-                _calibrator[compass_id].get_orientation(),
-                scale_factor
+                (uint8_t)cal_report.status, autosaved,
+                cal_report.fitness,
+                cal_report.ofs.x, cal_report.ofs.y, cal_report.ofs.z,
+                cal_report.diag.x, cal_report.diag.y, cal_report.diag.z,
+                cal_report.offdiag.x, cal_report.offdiag.y, cal_report.offdiag.z,
+                cal_report.orientation_confidence,
+                cal_report.original_orientation,
+                cal_report.orientation,
+                cal_report.scale_factor
             );
         }
     }
     return true;
 }
 
-bool Compass::is_calibrating() const
+bool Compass::is_calibrating()
 {
     for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        switch(_calibrator[i].get_status()) {
+        if (_calibrator[i] == nullptr) {
+            continue;
+        }
+        switch(_calibrator[i]->get_state().status) {
             case CompassCalibrator::Status::NOT_STARTED:
             case CompassCalibrator::Status::SUCCESS:
             case CompassCalibrator::Status::FAILED:
@@ -291,11 +319,14 @@ bool Compass::is_calibrating() const
     return false;
 }
 
-uint8_t Compass::_get_cal_mask() const
+uint8_t Compass::_get_cal_mask()
 {
     uint8_t cal_mask = 0;
     for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        if (_calibrator[i].get_status() != CompassCalibrator::Status::NOT_STARTED) {
+        if (_calibrator[i] == nullptr) {
+            continue;
+        }
+        if (_calibrator[i]->get_state().status != CompassCalibrator::Status::NOT_STARTED) {
             cal_mask |= 1 << uint8_t(i);
         }
     }

libraries/AP_Compass/CompassCalibrator.cpp

@@ -57,6 +57,7 @@
  * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
  */
 
+#include "AP_Compass.h"
 #include "CompassCalibrator.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_GeodesicGrid.h>
@@ -75,131 +76,111 @@ extern const AP_HAL::HAL& hal;
 
 CompassCalibrator::CompassCalibrator()
 {
-    stop();
+    set_status(Status::NOT_STARTED);
 }
 
+// Request to cancel calibration 
 void CompassCalibrator::stop()
 {
-    set_status(Status::NOT_STARTED);
+    WITH_SEMAPHORE(state_sem);
+    _requested_status = Status::NOT_STARTED;
+    _status_set_requested = true;
 }
 
 void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
 {
-    _check_orientation = true;
-    _orientation = orientation;
-    _orig_orientation = orientation;
-    _is_external = is_external;
-    _fix_orientation = fix_orientation;
+    WITH_SEMAPHORE(state_sem);
+    cal_settings.check_orientation = true;
+    cal_settings.orientation = orientation;
+    cal_settings.orig_orientation = orientation;
+    cal_settings.is_external = is_external;
+    cal_settings.fix_orientation = fix_orientation;
 }
 
-void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
+void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
 {
-    if (running()) {
+    if (compass_idx > COMPASS_MAX_INSTANCES) {
         return;
     }
-    _offset_max = offset_max;
-    _attempt = 1;
-    _retry = retry;
-    _delay_start_sec = delay;
-    _start_time_ms = AP_HAL::millis();
-    _compass_idx = compass_idx;
-    set_status(Status::WAITING_TO_START);
-}
 
-void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor)
-{
-    if (_status != Status::SUCCESS) {
+    WITH_SEMAPHORE(state_sem);
+    // Don't do this while we are already started
+    if (_running()) {
         return;
     }
-
-    offsets = _params.offset;
-    diagonals = _params.diag;
-    offdiagonals = _params.offdiag;
-    scale_factor = _params.scale_factor;
+    cal_settings.offset_max = offset_max;
+    cal_settings.attempt = 1;
+    cal_settings.retry = retry;
+    cal_settings.delay_start_sec = delay;
+    cal_settings.start_time_ms = AP_HAL::millis();
+    cal_settings.compass_idx = compass_idx;
+    cal_settings.tolerance = tolerance;
+
+    // Request status change to Waiting to start
+    _requested_status = Status::WAITING_TO_START;
+    _status_set_requested = true;
 }
 
-float CompassCalibrator::get_completion_percent() const
+// Record point mag sample and associated attitude sample to intermediate struct
+void CompassCalibrator::new_sample(const Vector3f& sample)
 {
-    // first sampling step is 1/3rd of the progress bar
-    // never return more than 99% unless _status is Status::SUCCESS
-    switch (_status) {
-        case Status::NOT_STARTED:
-        case Status::WAITING_TO_START:
-            return 0.0f;
-        case Status::RUNNING_STEP_ONE:
-            return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
-        case Status::RUNNING_STEP_TWO:
-            return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
-        case Status::SUCCESS:
-            return 100.0f;
-        case Status::FAILED:
-        case Status::BAD_ORIENTATION:
-        case Status::BAD_RADIUS:
-            return 0.0f;
-    };
-    // will not get here if the compiler is doing its job (no default clause)
-    return 0.0f;
+    WITH_SEMAPHORE(sample_sem);
+    _last_sample.set(sample);
+    _last_sample.att.set_from_ahrs();
+    _new_sample = true;
 }
 
-// update completion mask based on latest sample
-// used to ensure we have collected samples in all directions
-void CompassCalibrator::update_completion_mask(const Vector3f& v)
-{
-    Matrix3f softiron {
-        _params.diag.x,    _params.offdiag.x, _params.offdiag.y,
-        _params.offdiag.x, _params.diag.y,    _params.offdiag.z,
-        _params.offdiag.y, _params.offdiag.z, _params.diag.z
-    };
-    Vector3f corrected = softiron * (v + _params.offset);
-    int section = AP_GeodesicGrid::section(corrected, true);
-    if (section < 0) {
-        return;
-    }
-    _completion_mask[section / 8] |= 1 << (section % 8);
+bool CompassCalibrator::failed() {
+    WITH_SEMAPHORE(state_sem);
+    return (cal_state.status == Status::FAILED ||
+            cal_state.status == Status::BAD_ORIENTATION || 
+            cal_state.status == Status::BAD_RADIUS);
 }
 
-// reset and update the completion mask using all samples in the sample buffer
-void CompassCalibrator::update_completion_mask()
-{
-    memset(_completion_mask, 0, sizeof(_completion_mask));
-    for (int i = 0; i < _samples_collected; i++) {
-        update_completion_mask(_sample_buffer[i].get());
-    }
+
+bool CompassCalibrator::running() {
+    WITH_SEMAPHORE(state_sem); 
+    return (cal_state.status == Status::RUNNING_STEP_ONE || cal_state.status == Status::RUNNING_STEP_TWO);
 }
 
-bool CompassCalibrator::check_for_timeout()
-{
-    uint32_t tnow = AP_HAL::millis();
-    if (running() && tnow - _last_sample_ms > 1000) {
-        _retry = false;
-        set_status(Status::FAILED);
-        return true;
-    }
-    return false;
+const CompassCalibrator::Report CompassCalibrator::get_report() {
+    WITH_SEMAPHORE(state_sem);
+    return cal_report;
 }
 
-void CompassCalibrator::new_sample(const Vector3f& sample)
+const CompassCalibrator::State CompassCalibrator::get_state() {
+    WITH_SEMAPHORE(state_sem);
+    return cal_state;
+}
+/////////////////////////////////////////////////////////////
+////////////////////// PRIVATE METHODS //////////////////////
+/////////////////////////////////////////////////////////////
+
+void CompassCalibrator::update()
 {
-    _last_sample_ms = AP_HAL::millis();
 
-    if (_status == Status::WAITING_TO_START) {
-        set_status(Status::RUNNING_STEP_ONE);
-    }
+    //pickup samples from intermediate struct
+    pull_sample();
 
-    if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
-        update_completion_mask(sample);
-        _sample_buffer[_samples_collected].set(sample);
-        _sample_buffer[_samples_collected].att.set_from_ahrs();
-        _samples_collected++;
-    }
-}
+    {
+        WITH_SEMAPHORE(state_sem);
+        //update_settings
+        if (!running()) {
+            update_cal_settings();
+        }
 
-void CompassCalibrator::update(bool &failure)
-{
-    failure = false;
+        //update requested state
+        if (_status_set_requested) {
+            _status_set_requested = false;
+            set_status(_requested_status);
+        }
+        //update report and status
+        update_cal_status();
+        update_cal_report();
+    }
 
     // collect the minimum number of samples
-    if (!fitting()) {
+    if (!_fitting()) {
         return;
     }
 
@@ -207,7 +188,6 @@ void CompassCalibrator::update(bool &failure)
         if (_fit_step >= 10) {
             if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) {  // if true, means that fitness is diverging instead of converging
                 set_status(Status::FAILED);
-                failure = true;
             } else {
                 set_status(Status::RUNNING_STEP_TWO);
             }
@@ -224,7 +204,6 @@ void CompassCalibrator::update(bool &failure)
                 set_status(Status::SUCCESS);
             } else {
                 set_status(Status::FAILED);
-                failure = true;
             }
         } else if (_fit_step < 15) {
             run_sphere_fit();
@@ -236,17 +215,124 @@ void CompassCalibrator::update(bool &failure)
     }
 }
 
-/////////////////////////////////////////////////////////////
-////////////////////// PRIVATE METHODS //////////////////////
-/////////////////////////////////////////////////////////////
-bool CompassCalibrator::running() const
+void CompassCalibrator::pull_sample()
+{
+    CompassSample mag_sample;
+    {
+        WITH_SEMAPHORE(sample_sem);
+        if (!_new_sample) {
+            return;
+        }
+        if (_status == Status::WAITING_TO_START) {
+            set_status(Status::RUNNING_STEP_ONE);
+        }
+        _new_sample = false;
+        mag_sample = _last_sample;
+    }
+    if (_running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(mag_sample.get())) {
+        update_completion_mask(mag_sample.get());
+        _sample_buffer[_samples_collected] = mag_sample;
+        _samples_collected++;
+    }
+}
+
+
+void CompassCalibrator::update_cal_settings()
+{
+    _tolerance = cal_settings.tolerance;
+    _check_orientation = cal_settings.check_orientation;
+    _orientation = cal_settings.orientation;
+    _orig_orientation = cal_settings.orig_orientation;
+    _is_external = cal_settings.is_external;
+    _fix_orientation = cal_settings.fix_orientation;
+    _offset_max = cal_settings.offset_max;
+    _attempt = cal_settings.attempt;
+    _retry = cal_settings.retry;
+    _delay_start_sec = cal_settings.delay_start_sec;
+    _start_time_ms = cal_settings.start_time_ms;
+    _compass_idx = cal_settings.compass_idx;
+}
+
+// update completion mask based on latest sample
+// used to ensure we have collected samples in all directions
+void CompassCalibrator::update_completion_mask(const Vector3f& v)
+{
+    Matrix3f softiron {
+        _params.diag.x,    _params.offdiag.x, _params.offdiag.y,
+        _params.offdiag.x, _params.diag.y,    _params.offdiag.z,
+        _params.offdiag.y, _params.offdiag.z, _params.diag.z
+    };
+    Vector3f corrected = softiron * (v + _params.offset);
+    int section = AP_GeodesicGrid::section(corrected, true);
+    if (section < 0) {
+        return;
+    }
+    _completion_mask[section / 8] |= 1 << (section % 8);
+}
+
+// reset and update the completion mask using all samples in the sample buffer
+void CompassCalibrator::update_completion_mask()
+{
+    memset(_completion_mask, 0, sizeof(_completion_mask));
+    for (int i = 0; i < _samples_collected; i++) {
+        update_completion_mask(_sample_buffer[i].get());
+    }
+}
+
+void CompassCalibrator::update_cal_status()
+{
+    cal_state.status = _status;
+    cal_state.attempt = _attempt;
+    memcpy(cal_state.completion_mask, _completion_mask, sizeof(completion_mask_t));
+    cal_state.completion_pct = 0.0f;
+    // first sampling step is 1/3rd of the progress bar
+    // never return more than 99% unless _status is Status::SUCCESS
+    switch (_status) {
+        case Status::NOT_STARTED:
+        case Status::WAITING_TO_START:
+            cal_state.completion_pct = 0.0f;
+            break;
+        case Status::RUNNING_STEP_ONE:
+            cal_state.completion_pct = 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
+            break;
+        case Status::RUNNING_STEP_TWO:
+            cal_state.completion_pct = 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
+            break;
+        case Status::SUCCESS:
+            cal_state.completion_pct = 100.0f;
+            break;
+        case Status::FAILED:
+        case Status::BAD_ORIENTATION:
+        case Status::BAD_RADIUS:
+            cal_state.completion_pct = 0.0f;
+            break;
+    };
+}
+
+
+void CompassCalibrator::update_cal_report()
+{
+    cal_report.status = _status;
+    cal_report.fitness = sqrtf(_fitness);
+    cal_report.ofs = _params.offset;
+    cal_report.diag = _params.diag;
+    cal_report.offdiag = _params.offdiag;
+    cal_report.scale_factor = _params.scale_factor;
+    cal_report.orientation_confidence = _orientation_confidence;
+    cal_report.original_orientation = _orig_orientation;
+    cal_report.orientation = _orientation_solution;
+}
+
+// running method for use in thread
+bool CompassCalibrator::_running() const
 {
     return _status == Status::RUNNING_STEP_ONE || _status == Status::RUNNING_STEP_TWO;
 }
 
-bool CompassCalibrator::fitting() const
+// fitting method for use in thread
+bool CompassCalibrator::_fitting() const
 {
-    return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
+    return _running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
 }
 
 // initialize fitness before starting a fit
@@ -825,6 +911,8 @@ bool CompassCalibrator::calculate_orientation(void)
 
     float variance[ROTATION_MAX_AUTO_ROTATION+1] {};
 
+    _orientation_solution = _orientation;
+    
     for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
         // calculate the average implied earth field across all samples
         Vector3f total_ef {};
@@ -904,6 +992,7 @@ bool CompassCalibrator::calculate_orientation(void)
         // we won't change the orientation, but we set _orientation
         // for reporting purposes
         _orientation = besti;
+        _orientation_solution = besti;
         set_status(Status::BAD_ORIENTATION);
         return false;
     }
@@ -923,6 +1012,7 @@ bool CompassCalibrator::calculate_orientation(void)
     }
 
     _orientation = besti;
+    _orientation_solution = besti;
 
     // re-run the fit to get the diagonals and off-diagonals for the
     // new orientation

libraries/AP_Compass/CompassCalibrator.h

@@ -13,24 +13,25 @@ class CompassCalibrator {
 public:
     CompassCalibrator();
 
-    // set tolerance of calibration (aka fitness)
-    void set_tolerance(float tolerance) { _tolerance = tolerance; }
+    // start or stop the calibration
+    void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance);
+    void stop();
+
+    // Update point sample
+    void new_sample(const Vector3f& sample);
 
     // set compass's initial orientation and whether it should be automatically fixed (if required)
     void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation);
 
-    // start or stop the calibration
-    void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
-    void stop();
+    // running is true if actively calculating offsets, diagonals or offdiagonals
+    bool running();
 
-    // update the state machine and calculate offsets, diagonals and offdiagonals
-    void update(bool &failure);
-    void new_sample(const Vector3f &sample);
+    // failed is true if either of the failure states are hit
+    bool failed();
 
-    bool check_for_timeout();
 
-    // running is true if actively calculating offsets, diagonals or offdiagonals
-    bool running() const;
+    // update the state machine and calculate offsets, diagonals and offdiagonals
+    void update();
 
     // compass calibration states
     enum class Status {
@@ -44,27 +45,51 @@ public:
         BAD_RADIUS = 7,
     };
 
-    // get status of calibrations progress
-    Status get_status() const { return _status; }
-
-    // get calibration outputs (offsets, diagonals, offdiagonals) and fitness
-    void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor);
-    float get_fitness() const { return sqrtf(_fitness); }
-
-    // get corrected (and original) orientation
-    enum Rotation get_orientation() const { return _orientation; }
-    enum Rotation get_original_orientation() const { return _orig_orientation; }
-    float get_orientation_confidence() const { return _orientation_confidence; }
-
-    // get completion percentage (0 to 100) for reporting to GCS
-    float get_completion_percent() const;
-
-    // get how many attempts have been made to calibrate for reporting to GCS
-    uint8_t get_attempt() const { return _attempt; }
-
     // get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
     typedef uint8_t completion_mask_t[10];
-    const completion_mask_t& get_completion_mask() const { return _completion_mask; }
+
+    // Structure accessed for cal status update via mavlink 
+    struct State {
+        Status status;
+        uint8_t attempt;
+        float completion_pct;
+        completion_mask_t completion_mask;
+    } cal_state;
+
+    // Structure accessed after calibration is finished/failed
+    struct Report {
+        Status status;
+        float fitness;
+        Vector3f ofs;
+        Vector3f diag;
+        Vector3f offdiag;
+        float orientation_confidence;
+        Rotation original_orientation;
+        Rotation orientation;
+        float scale_factor;
+    } cal_report;
+
+    // Structure setup to set calibration run settings
+    struct Settings {
+        float tolerance;
+        bool check_orientation;
+        enum Rotation orientation;
+        enum Rotation orig_orientation;
+        bool is_external;
+        bool fix_orientation;
+        uint16_t offset_max;
+        uint8_t attempt;
+        bool retry;
+        float delay_start_sec;
+        uint32_t start_time_ms;
+        uint8_t compass_idx;
+    } cal_settings;
+
+    // Get calibration result
+    const Report get_report();
+    
+    // Get current Calibration state
+    const State get_state();
 
 private:
 
@@ -112,6 +137,9 @@ private:
     // set status including any required initialisation
     bool set_status(Status status);
 
+    // consume point raw sample from intermediate structure
+    void pull_sample();
+
     // returns true if sample should be added to buffer
     bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
     bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
@@ -126,7 +154,7 @@ private:
     void initialize_fit();
 
     // true if enough samples have been collected and fitting has begun (aka runniong())
-    bool fitting() const;
+    bool _fitting() const;
 
     // thins out samples between step one and step two
     void thin_samples();
@@ -162,9 +190,16 @@ private:
     // fix radius to compensate for sensor scaling errors
     bool fix_radius();
 
+    // update methods to read write intermediate structures, called inside thread
+    inline void update_cal_status();
+    inline void update_cal_report();
+    inline void update_cal_settings();
+
+    // running method for use in thread
+    bool _running() const;
+
     uint8_t _compass_idx;                   // index of the compass providing data
     Status _status;                         // current state of calibrator
-    uint32_t _last_sample_ms;               // system time of last sample received for timeout
 
     // values provided by caller
     float _delay_start_sec;                 // seconds to delay start of calibration (provided by caller)
@@ -191,8 +226,21 @@ private:
     // variables for orientation checking
     enum Rotation _orientation;             // latest detected orientation
     enum Rotation _orig_orientation;        // original orientation provided by caller
+    enum Rotation _orientation_solution;    // latest solution
     bool _is_external;                      // true if compass is external (provided by caller)
     bool _check_orientation;                // true if orientation should be automatically checked
     bool _fix_orientation;                  // true if orientation should be fixed if necessary
     float _orientation_confidence;          // measure of confidence in automatic orientation detection
+    CompassSample _last_sample;
+
+    Status _requested_status;
+    bool   _status_set_requested;
+
+    bool _new_sample;
+
+    // Semaphore for state related intermediate structures
+    HAL_Semaphore state_sem;
+
+    // Semaphore for intermediate structure for point sample collection
+    HAL_Semaphore sample_sem;
 };