Tracker: factor out compass functions

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;

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

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
 */

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