From 57a3bc1397239cbd66e5b32d59dcc467003e4bad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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 +#include -#include "AP_Compass.h" +#include +#include +#include -// don't allow any axis of the offset to go above 2000 -#define COMPASS_OFS_LIMIT 2000 +#include "Compass_learn.h" + +#include + +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; knew_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; iregister_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; ktake_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 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);