From 1adda1ccf0874e9372f27a54dde04f03fa20be09 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 27 Mar 2019 18:01:18 +1100
Subject: [PATCH] AP_Compass: move automatic declination setting into
 AP_Compass itself

---
 libraries/AP_Compass/AP_Compass.cpp | 31 ++++++++++++++++++++---------
 libraries/AP_Compass/AP_Compass.h   | 15 +++++++-------
 2 files changed, 30 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index fe7e894216..03d921dc12 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -959,6 +959,9 @@ void Compass::_detect_backends(void)
 bool
 Compass::read(void)
 {
+    if (!_initial_location_set) {
+        try_set_initial_location();
+    }
     for (uint8_t i=0; i< _backend_count; i++) {
         // call read on each of the backend. This call updates field[i]
         _backends[i]->read();
@@ -1060,18 +1063,28 @@ Compass::save_motor_compensation()
     }
 }
 
-void
-Compass::set_initial_location(int32_t latitude, int32_t longitude)
+void Compass::try_set_initial_location()
 {
+    if (!_auto_declination) {
+        return;
+    }
+    if (!_enabled) {
+        return;
+    }
+
+    Location loc;
+    if (!AP::ahrs().get_position(loc)) {
+        return;
+    }
+    _initial_location_set = true;
+
     // if automatic declination is configured, then compute
     // the declination based on the initial GPS fix
-    if (_auto_declination) {
-        // Set the declination based on the lat/lng from GPS
-        _declination.set(radians(
-                AP_Declination::get_declination(
-                    (float)latitude / 10000000,
-                    (float)longitude / 10000000)));
-    }
+    // Set the declination based on the lat/lng from GPS
+    _declination.set(radians(
+                         AP_Declination::get_declination(
+                             (float)loc.lat / 10000000,
+                             (float)loc.lng / 10000000)));
 }
 
 /// return true if the compass should be used for yaw calculations
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index fe68d386db..c6b502c24c 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -173,13 +173,6 @@ public:
 
     const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; }
     const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); }
-    
-    /// Sets the initial location used to get declination
-    ///
-    /// @param  latitude             GPS Latitude.
-    /// @param  longitude            GPS Longitude.
-    ///
-    void set_initial_location(int32_t latitude, int32_t longitude);
 
     // learn offsets accessor
     bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
@@ -482,6 +475,14 @@ private:
 
     CompassLearn *learn;
     bool learn_allocated;
+
+    /// Sets the initial location used to get declination
+    ///
+    /// @param  latitude             GPS Latitude.
+    /// @param  longitude            GPS Longitude.
+    ///
+    void try_set_initial_location();
+    bool _initial_location_set;
 };
 
 namespace AP {