From 2b19b8a988f51c3f7e0d3fa04ea4e2eaf7e148a0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 14 Oct 2021 19:48:47 +1100
Subject: [PATCH] AP_Compass: switched to GSF based compass learning

this makes inflight compass learning faster, more accurate and much
simpler
---
 libraries/AP_Compass/AP_Compass.h             |   3 +-
 .../AP_Compass/AP_Compass_Calibration.cpp     |   4 +-
 libraries/AP_Compass/Compass_learn.cpp        | 256 ++----------------
 libraries/AP_Compass/Compass_learn.h          |  44 +--
 4 files changed, 33 insertions(+), 274 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 5766ddf096..5efe23adb7 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -336,7 +336,8 @@ public:
       fast compass calibration given vehicle position and yaw
      */
     MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
-                                 float lat_deg, float lon_deg);
+                                 float lat_deg, float lon_deg,
+                                 bool force_use=false);
 
 #if HAL_MSP_COMPASS_ENABLED
     void handle_msp(const MSP::msp_compass_data_message_t &pkt);
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index e156ec433a..af05912c3c 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -483,7 +483,7 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
   This assumes that the compass is correctly scaled in milliGauss
 */
 MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
-                                      float lat_deg, float lon_deg)
+                                      float lat_deg, float lon_deg, bool force_use)
 {
     _reset_compass_id();
     if (is_zero(lat_deg) && is_zero(lon_deg)) {
@@ -526,7 +526,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
             // skip this compass
             continue;
         }
-        if (!use_for_yaw(i)) {
+        if (!force_use && !use_for_yaw(i)) {
             continue;
         }
         if (!healthy(i)) {
diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp
index f383db1f11..a9b3e9683c 100644
--- a/libraries/AP_Compass/Compass_learn.cpp
+++ b/libraries/AP_Compass/Compass_learn.cpp
@@ -1,268 +1,68 @@
-#include <AP_Math/AP_Math.h>
 #include <AP_AHRS/AP_AHRS.h>
 
 #include <AP_Compass/AP_Compass.h>
-#include <AP_Declination/AP_Declination.h>
-#include <AP_Logger/AP_Logger.h>
 
 #include "Compass_learn.h"
 #include <GCS_MAVLink/GCS.h>
-
-#include <stdio.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_NavEKF/EKFGSF_yaw.h>
 
 #if COMPASS_LEARN_ENABLED
 
 extern const AP_HAL::HAL &hal;
 
-static const uint8_t COMPASS_LEARN_NUM_SAMPLES = 30;
-static const uint8_t COMPASS_LEARN_BEST_ERROR_THRESHOLD = 50;
-static const uint8_t COMPASS_LEARN_WORST_ERROR_THRESHOLD = 65;
-
 // constructor
 CompassLearn::CompassLearn(Compass &_compass) :
     compass(_compass)
 {
     gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
-    for (Compass::Priority i(0); i<compass.get_count(); i++) {
-        if (compass._use_for_yaw[Compass::Priority(i)]) {
-            // reset scale factors, we can't learn scale factors in
-            // flight
-            compass.set_and_save_scale_factor(uint8_t(i), 0.0);
-        }
-    }
 }
 
+// accuracy threshold applied for GSF yaw estimate
+#define YAW_ACCURACY_THRESHOLD_DEG 5.0
+
 /*
   update when new compass sample available
  */
 void CompassLearn::update(void)
 {
     const AP_Vehicle *vehicle = AP::vehicle();
-    if (converged || compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
-        !hal.util->get_soft_armed() || vehicle->get_time_flying_ms() < 3000) {
+    if (compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
+        !hal.util->get_soft_armed() ||
+        vehicle->get_time_flying_ms() < 3000) {
         // only learn when flying and with enough time to be clear of
         // the ground
         return;
     }
 
-    const AP_AHRS &ahrs = AP::ahrs();
-    if (!have_earth_field) {
-        Location loc;
-        if (!ahrs.get_position(loc)) {
-            // need to wait till we have a global position
-            return;
-        }
-
-        // setup the expected earth field in mGauss at this location
-        mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000;
-        have_earth_field = true;
-
-        // form eliptical correction matrix and invert it. This is
-        // needed to remove the effects of the eliptical correction
-        // when calculating new offsets
-        const Vector3f &diagonals = compass.get_diagonals(0);
-        const Vector3f &offdiagonals = compass.get_offdiagonals(0);
-        mat = Matrix3f(
-            diagonals.x, offdiagonals.x, offdiagonals.y,
-            offdiagonals.x,    diagonals.y, offdiagonals.z,
-            offdiagonals.y, offdiagonals.z,    diagonals.z
-            );
-        if (!mat.invert()) {
-            // if we can't invert, use the identity matrix
-            mat.identity();
-        }
-
-        // set initial error to field intensity
-        float intensity = mag_ef.length();
-        for (uint16_t i=0; i<num_sectors; i++) {
-            errors[i] = intensity;
-        }
-
-        gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field");
-        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
+    const auto &ahrs = AP::ahrs();
+    const auto *gsf = ahrs.get_yaw_estimator();
+    if (gsf == nullptr) {
+        // no GSF available
+        return;
+    }
+    if (degrees(fabsf(ahrs.get_pitch())) > 50) {
+        // we don't want to be too close to nose up, or yaw gets
+        // problematic. Tailsitters need to wait till they are in
+        // forward flight
+        return;
     }
 
     AP_Notify::flags.compass_cal_running = true;
 
-    if (sample_available) {
-        // last sample still being processed by IO thread
+    ftype yaw_rad, yaw_variance;
+    if (!gsf->getYawData(yaw_rad, yaw_variance) ||
+        !is_positive(yaw_variance) ||
+        yaw_variance >= sq(radians(YAW_ACCURACY_THRESHOLD_DEG))) {
+        // not converged
         return;
     }
 
-    Vector3f field = compass.get_field(0);
-    Vector3f field_change = field - last_field;
-    if (field_change.length() < min_field_change) {
-        return;
-    }
-
-    {
-        WITH_SEMAPHORE(sem);
-        // give a sample to the backend to process
-        new_sample.field = field;
-        new_sample.offsets = compass.get_offsets(0);
-        new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw);
-        sample_available = true;
-        last_field = field;
-        num_samples++;
-    }
-
-    if (sample_available) {
-        // @LoggerMessage: COFS
-        // @Description: Current compass learn offsets
-        // @Field: TimeUS: Time since system startup
-        // @Field: OfsX: best learnt offset, x-axis
-        // @Field: OfsY: best learnt offset, y-axis
-        // @Field: OfsZ: best learnt offset, z-axis
-        // @Field: Var: error of best offset vector
-        // @Field: Yaw: best learnt yaw
-        // @Field: WVar: error of best learn yaw
-        // @Field: N: number of samples used
-        AP::logger().WriteStreaming("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
-                                               AP_HAL::micros64(),
-                                               (double)best_offsets.x,
-                                               (double)best_offsets.y,
-                                               (double)best_offsets.z,
-                                               (double)best_error,
-                                               (double)best_yaw_deg,
-                                               (double)worst_error,
-                                               num_samples);
-    }
-
-    if (!converged) {
-        WITH_SEMAPHORE(sem);
-
-        // set offsets to current best guess
-        compass.set_offsets(0, best_offsets);
-
-        // set non-primary offsets to match primary
-        Vector3f field_primary = compass.get_field(0);
-        for (uint8_t i=1; i<compass.get_count(); i++) {
-            if (!compass._use_for_yaw[Compass::Priority(i)]) {
-                continue;
-            }
-            Vector3f field2 = compass.get_field(i);
-            Vector3f new_offsets = compass.get_offsets(i) + (field_primary - field2);
-            compass.set_offsets(i, new_offsets);
-        }
-
-        // stop updating the offsets once converged
-        if (num_samples > COMPASS_LEARN_NUM_SAMPLES &&
-            best_error < COMPASS_LEARN_BEST_ERROR_THRESHOLD &&
-            worst_error > COMPASS_LEARN_WORST_ERROR_THRESHOLD) {
-            // set the offsets and enable compass for EKF use. Let the
-            // EKF learn the remaining compass offset error
-            for (uint8_t i=0; i<compass.get_count(); i++) {
-                if (compass._use_for_yaw[Compass::Priority(i)]) {
-                    compass.save_offsets(i);
-                    compass.set_and_save_scale_factor(i, 0.0);
-                }
-            }
-            compass.set_learn_type(Compass::LEARN_NONE, true);
-            // setup so use can trigger it again
-            converged = false;
-            sample_available = false;
-            num_samples = 0;
-            have_earth_field = false;
-            for (auto &v : predicted_offsets) {
-                v.zero();
-            }
-            worst_error = 0;
-            best_error = 0;
-            best_yaw_deg = 0;
-            best_offsets.zero();
-            gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: finished");
-            AP_Notify::flags.compass_cal_running = false;
-            AP_Notify::events.compass_cal_saved = true;
-        }
-    }
-}
-
-/*
-  we run the math intensive calculations in the IO thread
- */
-void CompassLearn::io_timer(void)
-{
-    if (!sample_available) {
-        return;
-    }
-
-    struct sample s;
-
-    {
-        WITH_SEMAPHORE(sem);
-        s = new_sample;
-        sample_available = false;
-    }
-
-    process_sample(s);
-}
-
-/*
-  process a new compass sample
- */
-void CompassLearn::process_sample(const struct sample &s)
-{
-    uint16_t besti = 0;
-    float bestv = 0, worstv=0;
-
-    /*
-      we run through the 72 possible yaw error values, and for each
-      one we calculate a value for the compass offsets if that yaw
-      error is correct. 
-     */
-    for (uint16_t i=0; i<num_sectors; i++) {
-        float yaw_err_deg = i*(360/num_sectors);
-
-        // form rotation matrix for the euler attitude
-        Matrix3f dcm;
-        dcm.from_euler(s.attitude.x, s.attitude.y, wrap_2PI(s.attitude.z + radians(yaw_err_deg)));
-
-        // calculate the field we would expect to get if this yaw error is correct
-        Vector3f expected_field = dcm.transposed() * mag_ef;
-
-        // calculate a value for the compass offsets for this yaw error
-        Vector3f v1 = mat * s.field;
-        Vector3f v2 = mat * expected_field;
-        Vector3f offsets = (v2 - v1) + s.offsets;
-        float delta = (offsets - predicted_offsets[i]).length();
-
-        if (num_samples == 1) {
-            predicted_offsets[i] = offsets;
-        } else {
-            // lowpass the predicted offsets and the error
-            const float learn_rate = 0.92f;
-            predicted_offsets[i] = predicted_offsets[i] * learn_rate + offsets * (1-learn_rate);
-            errors[i] = errors[i] * learn_rate + delta * (1-learn_rate);
-        }
-
-        // keep track of the current best prediction
-        if (i == 0 || errors[i] < bestv) {
-            besti = i;
-            bestv = errors[i];
-        }
-        // also keep the worst error. This is used as part of the convergence test
-        if (i == 0 || errors[i] > worstv) {
-            worstv = errors[i];
-        }
-    }
-
-    WITH_SEMAPHORE(sem);
-
-    // pass the current estimate to the front-end
-    best_offsets = predicted_offsets[besti];
-    best_error = bestv;
-    worst_error = worstv;
-    best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
-
-    // send current learn state to gcs
-    const uint32_t now = AP_HAL::millis();
-    if (!converged && now - last_learn_progress_sent_ms >= 5000) {
-        float percent = (MIN(num_samples / COMPASS_LEARN_NUM_SAMPLES, 1.0f) + 
-                         MIN(COMPASS_LEARN_BEST_ERROR_THRESHOLD / (best_error + 1.0f), 1.0f) + 
-                         MIN(worst_error / COMPASS_LEARN_WORST_ERROR_THRESHOLD, 1.0f)) / 3.0f * 100.f;
-        gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: %d%%", (int) percent);
-        last_learn_progress_sent_ms = now;
+    const auto result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
+    if (result == MAV_RESULT_ACCEPTED) {
+        AP_Notify::flags.compass_cal_running = false;
+        compass.set_learn_type(Compass::LEARN_NONE, true);
+        gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Finished");
     }
 }
 
diff --git a/libraries/AP_Compass/Compass_learn.h b/libraries/AP_Compass/Compass_learn.h
index 42acb1bcc0..f24de57fef 100644
--- a/libraries/AP_Compass/Compass_learn.h
+++ b/libraries/AP_Compass/Compass_learn.h
@@ -3,7 +3,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 
 /*
-  compass learning using magnetic field tables from AP_Declination
+  compass learning using magnetic field tables from AP_Declination and GSF
  */
 
 class CompassLearn {
@@ -15,46 +15,4 @@ public:
 
 private:
     Compass &compass;
-    bool have_earth_field;
-
-    // 5 degree resolution
-    static const uint16_t num_sectors = 72; 
-    
-    Vector3f predicted_offsets[num_sectors];
-    float errors[num_sectors];
-    uint32_t num_samples;
-
-    // earth field
-    Vector3f mag_ef;
-
-    // semaphore for access to shared data with IO thread
-    HAL_Semaphore sem;
-    
-    struct sample {
-        // milliGauss body field and offsets
-        Vector3f field;
-        Vector3f offsets;
-
-        // euler radians attitude
-        Vector3f attitude;
-    };
-
-    Matrix3f mat;
-    
-    struct sample new_sample;
-    bool sample_available;
-    Vector3f last_field;
-    static const uint32_t min_field_change = 60;
-
-    Vector3f best_offsets;
-    float best_error;
-    float best_yaw_deg;
-    float worst_error;
-    bool converged;
-
-    // notification
-    uint32_t last_learn_progress_sent_ms;
-
-    void io_timer(void);
-    void process_sample(const struct sample &s);
 };