AP_Compass: enable in-flight compass learning system

using COMPASS_LEARN=3

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();
 }
 

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 {

libraries/AP_Compass/Compass_learn.cpp

@@ -6,16 +6,17 @@
 #include <DataFlash/DataFlash.h>
 
 #include "Compass_learn.h"
+#include <GCS_MAVLink/GCS.h>
 
 #include <stdio.h>
 
 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");
         }
     }
 }

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;