From 57a3bc1397239cbd66e5b32d59dcc467003e4bad Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 18 Aug 2017 18:58:08 +1000
Subject: [PATCH] AP_Compass: new compass learning system

this learns compass offsets using magnetic tables and compass
observations
---
 libraries/AP_Compass/AP_Compass.cpp           |   5 +-
 libraries/AP_Compass/AP_Compass.h             |  22 +-
 libraries/AP_Compass/Compass_learn.cpp        | 296 +++++++++++-------
 libraries/AP_Compass/Compass_learn.h          |  57 ++++
 .../AP_Compass_test/AP_Compass_test.cpp       |   1 -
 5 files changed, 263 insertions(+), 118 deletions(-)
 create mode 100644 libraries/AP_Compass/Compass_learn.h

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 0458e54abb..4e72e406ac 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -1011,7 +1011,10 @@ Compass::use_for_yaw(void) const
 bool
 Compass::use_for_yaw(uint8_t i) const
 {
-    return _state[i].use_for_yaw;
+    // when we are doing in-flight compass learning the state
+    // estimator must not use the compass. The learning code turns off
+    // inflight learning when it has converged
+    return _state[i].use_for_yaw && _learn.get() != LEARN_INFLIGHT;
 }
 
 void
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 7d8f957758..2bad3dcd02 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -52,6 +52,8 @@ public:
     Compass(const Compass &other) = delete;
     Compass &operator=(const Compass&) = delete;
 
+    friend class CompassLearn;
+
     /// Initialize the compass device.
     ///
     /// @returns    True if the compass was initialized OK, false if it was not
@@ -182,14 +184,14 @@ public:
     // learn offsets accessor
     bool learn_offsets_enabled() const { return _learn; }
 
-    /// Perform automatic offset updates
-    ///
-    void learn_offsets(void);
-
     /// return true if the compass should be used for yaw calculations
     bool use_for_yaw(uint8_t i) const;
     bool use_for_yaw(void) const;
 
+    void set_use_for_yaw(uint8_t i, bool use) {
+        _state[i].use_for_yaw.set(use);
+    }
+
     /// Sets the local magnetic field declination.
     ///
     /// @param  radians             Local field declination.
@@ -301,7 +303,8 @@ public:
     enum LearnType {
         LEARN_NONE=0,
         LEARN_INTERNAL=1,
-        LEARN_EKF=2
+        LEARN_EKF=2,
+        LEARN_INFLIGHT=3
     };
 
     // return the chosen learning type
@@ -309,6 +312,15 @@ public:
         return (enum LearnType)_learn.get();
     }
 
+    // set the learning type
+    void set_learn_type(enum LearnType type, bool save) {
+        if (save) {
+            _learn.set_and_save((int8_t)type);
+        } else {
+            _learn.set((int8_t)type);
+        }
+    }
+    
     // return maximum allowed compass offsets
     uint16_t get_offsets_max(void) const {
         return (uint16_t)_offset_max.get();
diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp
index 43d77380a7..04032fe701 100644
--- a/libraries/AP_Compass/Compass_learn.cpp
+++ b/libraries/AP_Compass/Compass_learn.cpp
@@ -1,129 +1,203 @@
 #include <AP_Math/AP_Math.h>
+#include <AP_AHRS/AP_AHRS.h>
 
-#include "AP_Compass.h"
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Declination/AP_Declination.h>
+#include <DataFlash/DataFlash.h>
 
-// don't allow any axis of the offset to go above 2000
-#define COMPASS_OFS_LIMIT 2000
+#include "Compass_learn.h"
+
+#include <stdio.h>
+
+extern const AP_HAL::HAL &hal;
+
+// constructor
+CompassLearn::CompassLearn(AP_AHRS &_ahrs, Compass &_compass) :
+    ahrs(_ahrs),
+    compass(_compass)
+{
+}
 
 /*
- *  this offset learning algorithm is inspired by this paper from Bill Premerlani
- *
- *  http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
- *
- *  The base algorithm works well, but is quite sensitive to
- *  noise. After long discussions with Bill, the following changes were
- *  made:
- *
- *   1) we keep a history buffer that effectively divides the mag
- *      vectors into a set of N streams. The algorithm is run on the
- *      streams separately
- *
- *   2) within each stream we only calculate a change when the mag
- *      vector has changed by a significant amount.
- *
- *  This gives us the property that we learn quickly if there is no
- *  noise, but still learn correctly (and slowly) in the face of lots of
- *  noise.
+  update when new compass sample available
  */
-void
-Compass::learn_offsets(void)
+void CompassLearn::update(void)
 {
-    if (_learn == 0) {
-        // auto-calibration is disabled
+    if (converged || compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
+        !hal.util->get_soft_armed() || ahrs.get_time_flying_ms() < 3000) {
+        // only learn when flying and with enough time to be clear of
+        // the ground
         return;
     }
 
-    // this gain is set so we converge on the offsets in about 5
-    // minutes with a 10Hz compass
-    const float gain = 0.01f;
-    const float max_change = 10.0f;
-    const float min_diff = 50.0f;
-
-    if (!_null_init_done) {
-        // first time through
-        _null_init_done = true;
-        for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
-            const Vector3f &field = _state[k].field;
-            const Vector3f &ofs = _state[k].offset.get();
-            for (uint8_t i=0; i<_mag_history_size; i++) {
-                // fill the history buffer with the current mag vector,
-                // with the offset removed
-                _state[k].mag_history[i] = Vector3i(roundf(field.x) - ofs.x, 
-                                                    roundf(field.y) - ofs.y, 
-                                                    roundf(field.z) - ofs.z);
-            }
-            _state[k].mag_history_index = 0;
+    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 at this location
+        float declination_deg=0, inclination_deg=0, intensity_gauss=0;
+        AP_Declination::get_mag_field_ef(loc.lat*1.0e-7, loc.lng*1.0e-7, intensity_gauss, declination_deg, inclination_deg);
+
+        // create earth field
+        mag_ef = Vector3f(intensity_gauss*1000, 0.0, 0.0);
+        Matrix3f R;
+
+        R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg));
+        mag_ef = R * mag_ef;
+
+        sem = hal.util->new_semaphore();
+
+        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
+        for (uint16_t i=0; i<num_sectors; i++) {
+            errors[i] = intensity_gauss*1000;
+        }
+        
+        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
+    }
+
+    if (sample_available) {
+        // last sample still being processed by IO thread
         return;
     }
 
-    for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
-        const Vector3f &ofs = _state[k].offset.get();
-        const Vector3f &field = _state[k].field;
-        Vector3f b1, diff;
-        float length;
+    Vector3f field = compass.get_field(0);
+    Vector3f field_change = field - last_field;
+    if (field_change.length() < min_field_change) {
+        return;
+    }
+    
+    if (sem->take_nonblocking()) {
+        // 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++;
+        sem->give();
+    }
 
-        if (ofs.is_nan()) {
-            // offsets are bad possibly due to a past bug - zero them
-            _state[k].offset.set(Vector3f());
+    if (sample_available) {
+        DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
+                                               AP_HAL::micros64(),
+                                               best_offsets.x,
+                                               best_offsets.y,
+                                               best_offsets.z,
+                                               best_error,
+                                               best_yaw_deg,
+                                               worst_error,
+                                               num_samples);
+    }
+
+    if (!converged && sem->take_nonblocking()) {
+        // stop updating the offsets once converged
+        compass.set_offsets(0, best_offsets);
+        if (num_samples > 30 && best_error < 50 && worst_error > 65) {
+            // set the offsets and enable compass for EKF use. Let the
+            // EKF learn the remaining compass offset error
+            compass.save_offsets(0);
+            compass.set_use_for_yaw(0, true);
+            compass.set_learn_type(Compass::LEARN_EKF, true);
+            converged = true;
         }
-
-        // get a past element
-        b1 = Vector3f(_state[k].mag_history[_state[k].mag_history_index].x,
-                      _state[k].mag_history[_state[k].mag_history_index].y,
-                      _state[k].mag_history[_state[k].mag_history_index].z);
-
-        // the history buffer doesn't have the offsets
-        b1 += ofs;
-
-        // get the current vector
-        const Vector3f &b2 = field;
-
-        // calculate the delta for this sample
-        diff = b2 - b1;
-        length = diff.length();
-        if (length < min_diff) {
-            // the mag vector hasn't changed enough - we don't get
-            // enough information from this vector to use it.
-            // Note that we don't put the current vector into the mag
-            // history here. We want to wait for a larger rotation to
-            // build up before calculating an offset change, as accuracy
-            // of the offset change is highly dependent on the size of the
-            // rotation.
-            _state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
-            continue;
-        }
-
-        // put the vector in the history
-        _state[k].mag_history[_state[k].mag_history_index] = Vector3i(roundf(field.x) - ofs.x, 
-                                                                      roundf(field.y) - ofs.y, 
-                                                                      roundf(field.z) - ofs.z);
-        _state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
-
-        // equation 6 of Bills paper
-        diff = diff * (gain * (b2.length() - b1.length()) / length);
-
-        // limit the change from any one reading. This is to prevent
-        // single crazy readings from throwing off the offsets for a long
-        // time
-        length = diff.length();
-        if (length > max_change) {
-            diff *= max_change / length;
-        }
-
-        Vector3f new_offsets = _state[k].offset.get() - diff;
-
-        if (new_offsets.is_nan()) {
-            // don't apply bad offsets
-            continue;
-        }
-
-        // constrain offsets
-        new_offsets.x = constrain_float(new_offsets.x, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
-        new_offsets.y = constrain_float(new_offsets.y, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
-        new_offsets.z = constrain_float(new_offsets.z, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
-            
-        // set the new offsets
-        _state[k].offset.set(new_offsets);
+        sem->give();
+    }
+}
+
+/*
+  we run the math intensive calculations in the IO thread
+ */
+void CompassLearn::io_timer(void)
+{
+    if (!sample_available) {
+        return;
+    }
+    struct sample s;
+    if (!sem->take_nonblocking()) {
+        return;
+    }
+    s = new_sample;
+    sample_available = false;
+    sem->give();
+
+    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.92;
+            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];
+        }
+    }
+
+    if (sem->take_nonblocking()) {
+        // 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));
+        sem->give();
     }
 }
diff --git a/libraries/AP_Compass/Compass_learn.h b/libraries/AP_Compass/Compass_learn.h
new file mode 100644
index 0000000000..2002866008
--- /dev/null
+++ b/libraries/AP_Compass/Compass_learn.h
@@ -0,0 +1,57 @@
+#pragma once
+
+/*
+  compass learning using magnetic field tables from AP_Declination
+ */
+
+class CompassLearn {
+public:
+    CompassLearn(AP_AHRS &ahrs, Compass &compass);
+
+    // called on each compass read
+    void update(void);
+
+private:
+    // reference to AHRS library. Needed for attitude, position and compass
+    const AP_AHRS &ahrs;
+    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
+    AP_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;
+
+    void io_timer(void);
+    void process_sample(const struct sample &s);
+};
diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
index e5ec46e7c2..a6484485a9 100644
--- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
+++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
@@ -62,7 +62,6 @@ static void loop()
             // use roll = 0, pitch = 0 for this example
             dcm_matrix.from_euler(0, 0, 0);
             heading = compass.calculate_heading(dcm_matrix, i);
-            compass.learn_offsets();
 
             const Vector3f &mag = compass.get_field(i);