AP_Compass: implement completion mask

Fill the completion mask and send that through MAVLink while calibrating the
compass.

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -182,23 +182,22 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
     uint8_t cal_mask = get_cal_mask();
 
     for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
-        uint8_t cal_status = _calibrator[compass_id].get_status();
+        auto& calibrator = _calibrator[compass_id];
+        uint8_t cal_status = calibrator.get_status();
 
         if (cal_status == COMPASS_CAL_WAITING_TO_START  ||
             cal_status == COMPASS_CAL_RUNNING_STEP_ONE ||
             cal_status == COMPASS_CAL_RUNNING_STEP_TWO) {
-            uint8_t completion_pct = _calibrator[compass_id].get_completion_percent();
-            uint8_t completion_mask[10];
+            uint8_t completion_pct = calibrator.get_completion_percent();
+            auto& completion_mask = calibrator.get_completion_mask();
             Vector3f direction(0.0f,0.0f,0.0f);
             uint8_t attempt = _calibrator[compass_id].get_attempt();
 
-            memset(completion_mask, 0, sizeof(completion_mask));
-
             // 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,

libraries/AP_Compass/CompassCalibrator.cpp

@@ -60,6 +60,7 @@
 
 #include "CompassCalibrator.h"
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_GeodesicGrid.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -119,6 +120,34 @@ float CompassCalibrator::get_completion_percent() const {
     };
 }
 
+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);
+}
+
+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());
+    }
+}
+
+CompassCalibrator::completion_mask_t& CompassCalibrator::get_completion_mask()
+{
+    return _completion_mask;
+}
+
 bool CompassCalibrator::check_for_timeout() {
     uint32_t tnow = AP_HAL::millis();
     if(running() && tnow - _last_sample_ms > 1000) {
@@ -137,6 +166,7 @@ void CompassCalibrator::new_sample(const Vector3f& sample) {
     }
 
     if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
+        update_completion_mask(sample);
         _sample_buffer[_samples_collected].set(sample);
         _samples_collected++;
     }
@@ -210,6 +240,7 @@ void CompassCalibrator::reset_state() {
     _params.diag = Vector3f(1.0f,1.0f,1.0f);
     _params.offdiag.zero();
 
+    memset(_completion_mask, 0, sizeof(_completion_mask));
     initialize_fit();
 }
 
@@ -344,6 +375,8 @@ void CompassCalibrator::thin_samples() {
             _samples_thinned ++;
         }
     }
+
+    update_completion_mask();
 }
 
 /*
@@ -518,6 +551,7 @@ void CompassCalibrator::run_sphere_fit()
     if(!isnan(fitness) && fitness < _fitness) {
         _fitness = fitness;
         _params = fit1_params;
+        update_completion_mask();
     }
 }
 
@@ -634,6 +668,7 @@ void CompassCalibrator::run_ellipsoid_fit()
     if(fitness < _fitness) {
         _fitness = fitness;
         _params = fit1_params;
+        update_completion_mask();
     }
 }
 

libraries/AP_Compass/CompassCalibrator.h

@@ -18,6 +18,8 @@ enum compass_cal_status_t {
 
 class CompassCalibrator {
 public:
+    typedef uint8_t completion_mask_t[10];
+
     CompassCalibrator();
 
     void start(bool retry=false, bool autosave=false, float delay=0.0f);
@@ -35,6 +37,7 @@ public:
     void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
 
     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); }
     bool get_autosave() const { return _autosave; }
@@ -82,6 +85,8 @@ private:
     float _tolerance;
     uint8_t _attempt;
 
+    completion_mask_t _completion_mask;
+
     //fit state
     class param_t _params;
     uint16_t _fit_step;
@@ -120,5 +125,17 @@ private:
     void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
     void run_ellipsoid_fit();
 
+    /**
+     * Update #_completion_mask for the geodesic section of \p v. Corrections
+     * are applied to \p v with #_params.
+     *
+     * @param v[in] A vector representing one calibration sample.
+     */
+    void update_completion_mask(const Vector3f& v);
+    /**
+     * Reset and update #_completion_mask with the current samples.
+     */
+    void update_completion_mask();
+
     uint16_t get_random();
 };