From 1bade761a134d90912b73f36c2c101ffd8152d5f Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Fri, 8 Feb 2019 19:22:08 +0000
Subject: [PATCH] Tracker: factor out compass functions

---
 AntennaTracker/AntennaTracker.cpp |  7 +++---
 AntennaTracker/Tracker.h          |  6 +++++
 AntennaTracker/sensors.cpp        | 38 +++++++++++++++++++++++++++++++
 AntennaTracker/system.cpp         | 10 ++------
 4 files changed, 50 insertions(+), 11 deletions(-)

diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp
index c84adad7a6..928cbd3d69 100644
--- a/AntennaTracker/AntennaTracker.cpp
+++ b/AntennaTracker/AntennaTracker.cpp
@@ -101,12 +101,13 @@ void Tracker::one_second_loop()
     one_second_counter++;
 
     if (one_second_counter >= 60) {
-        if (g.compass_enabled) {
-            compass.save_offsets();
-        }
+        compass_save();
         one_second_counter = 0;
     }
 
+    // init compass location for declination
+    init_compass_location();
+
     if (!ahrs.home_is_set()) {
         // set home to current location
         Location temp_loc;
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index a8f3afd990..a9e07ed1ee 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -193,6 +193,9 @@ private:
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];
 
+     // true if the compass's initial location has been set
+    bool compass_init_location;
+
     // AntennaTracker.cpp
     void one_second_loop();
     void ten_hz_logging_loop();
@@ -231,6 +234,9 @@ private:
 
     // sensors.cpp
     void update_ahrs();
+    void init_compass();
+    void compass_save();
+    void init_compass_location();
     void update_compass(void);
     void compass_cal_update();
     void accel_cal_update(void);
diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp
index a725bfe1b3..917c8b2ab7 100644
--- a/AntennaTracker/sensors.cpp
+++ b/AntennaTracker/sensors.cpp
@@ -8,6 +8,35 @@ void Tracker::update_ahrs()
     ahrs.update();
 }
 
+// initialise compass
+void Tracker::init_compass()
+{
+    if (!g.compass_enabled) {
+        return;
+    }
+
+    if (!compass.init()|| !compass.read()) {
+        hal.console->printf("Compass initialisation failed!\n");
+        g.compass_enabled = false;
+    } else {
+        ahrs.set_compass(&compass);
+    }
+}
+
+/*
+  initialise compass's location used for declination
+ */
+void Tracker::init_compass_location(void)
+{
+    // update initial location used for declination
+    if (!compass_init_location) {
+        Location loc;
+        if (ahrs.get_position(loc)) {
+            compass.set_initial_location(loc.lat, loc.lng);
+            compass_init_location = true;
+        }
+    }
+}
 
 /*
   read and update compass
@@ -31,6 +60,15 @@ void Tracker::compass_cal_update() {
     }
 }
 
+// Save compass offsets
+void Tracker::compass_save() {
+    if (g.compass_enabled &&
+        compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
+        !hal.util->get_soft_armed()) {
+        compass.save_offsets();
+    }
+}
+
 /*
     Accel calibration
 */
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index ad2627b447..e6666b6ffd 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -53,14 +53,8 @@ void Tracker::init_tracker()
     log_init();
 #endif
 
-    if (g.compass_enabled==true) {
-        if (!compass.init() || !compass.read()) {
-            hal.console->printf("Compass initialisation failed!\n");
-            g.compass_enabled = false;
-        } else {
-            ahrs.set_compass(&compass);
-        }
-    }
+    // initialise compass
+    init_compass();
 
     // GPS Initialization
     gps.set_log_gps_bit(MASK_LOG_GPS);