diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 6927b25855..a1bf48c0dc 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -23,6 +23,7 @@ #include "AP_Compass_MMC3416.h" #include "AP_Compass_MAG3110.h" #include "AP_Compass.h" +#include "Compass_learn.h" extern AP_HAL::HAL& hal; @@ -968,6 +969,13 @@ Compass::read(void) for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { _state[i].healthy = (time - _state[i].last_update_ms < 500); } + if (_learn == LEARN_INFLIGHT && !learn_allocated) { + learn_allocated = true; + learn = new CompassLearn(*this); + } + if (_learn == LEARN_INFLIGHT && learn != nullptr) { + learn->update(); + } return healthy(); } diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 693ff374c1..75c8eda30e 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -46,6 +46,8 @@ #define COMPASS_MAX_INSTANCES 3 #define COMPASS_MAX_BACKEND 3 +class CompassLearn; + class Compass { friend class AP_Compass_Backend; @@ -187,7 +189,7 @@ public: } // learn offsets accessor - bool learn_offsets_enabled() const { return _learn; } + bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; } /// return true if the compass should be used for yaw calculations bool use_for_yaw(uint8_t i) const; @@ -476,6 +478,9 @@ private: AP_Int32 _driver_type_mask; AP_Int8 _filter_range; + + CompassLearn *learn; + bool learn_allocated; }; namespace AP { diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index 5bd31168e3..df3590ce6b 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -6,16 +6,17 @@ #include #include "Compass_learn.h" +#include #include extern const AP_HAL::HAL &hal; // constructor -CompassLearn::CompassLearn(AP_AHRS &_ahrs, Compass &_compass) : - ahrs(_ahrs), +CompassLearn::CompassLearn(Compass &_compass) : compass(_compass) { + gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised"); } /* @@ -23,6 +24,7 @@ CompassLearn::CompassLearn(AP_AHRS &_ahrs, Compass &_compass) : */ void CompassLearn::update(void) { + const AP_AHRS &ahrs = AP::ahrs(); 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 @@ -70,6 +72,7 @@ void CompassLearn::update(void) errors[i] = intensity_gauss*1000; } + gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field"); hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void)); } @@ -119,6 +122,7 @@ void CompassLearn::update(void) compass.set_use_for_yaw(0, true); compass.set_learn_type(Compass::LEARN_EKF, true); converged = 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 97f2816a14..9792ee4a46 100644 --- a/libraries/AP_Compass/Compass_learn.h +++ b/libraries/AP_Compass/Compass_learn.h @@ -8,14 +8,12 @@ class CompassLearn { public: - CompassLearn(AP_AHRS &ahrs, Compass &compass); + CompassLearn(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;