From eb2aa80fca1975df8afb301f3f92dc14c8746b61 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 7 Jun 2017 10:26:51 +0900
Subject: [PATCH] Rover: compass set-initial-location uses ahrs location

Previously it could attempt to use a gps location even if gps was not being used
Also compass-accumulate moved to sensors.cpp
---
 APMrover2/APMrover2.cpp | 10 ----------
 APMrover2/Rover.h       |  3 +++
 APMrover2/commands.cpp  |  4 ----
 APMrover2/sensors.cpp   | 22 ++++++++++++++++++++++
 4 files changed, 25 insertions(+), 14 deletions(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 4518139433..6752d73019 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -235,16 +235,6 @@ void Rover::gcs_failsafe_check(void)
     }
 }
 
-/*
-  if the compass is enabled then try to accumulate a reading
- */
-void Rover::compass_accumulate(void)
-{
-    if (g.compass_enabled) {
-        compass.accumulate();
-    }
-}
-
 /*
   check for new compass data - 10Hz
  */
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index b0b1828de0..198e354218 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -334,6 +334,9 @@ private:
     // true if the system time has been set from the GPS
     bool system_time_set;
 
+    // true if the compass's initial location has been set
+    bool compass_init_location;
+
     // The location of the previous waypoint.  Used for track following and altitude ramp calculations
     struct Location prev_WP;
     // The location of the current/active waypoint.  Used for track following
diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp
index 982de5bc3b..9e75552773 100644
--- a/APMrover2/commands.cpp
+++ b/APMrover2/commands.cpp
@@ -106,10 +106,6 @@ bool Rover::set_home(const Location& loc, bool lock)
 
     // init compass declination
     if (home_is_set == HOME_UNSET) {
-        // Set compass declination automatically
-        if (g.compass_enabled) {
-            compass.set_initial_location(loc.lat, loc.lng);
-        }
         // record home is set
         home_is_set = HOME_SET_NOT_LOCKED;
 
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index 561576e09e..c588977914 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -1,5 +1,27 @@
 #include "Rover.h"
 
+/*
+  if the compass is enabled then try to accumulate a reading
+  also update initial location used for declination
+ */
+void Rover::compass_accumulate(void)
+{
+    if (!g.compass_enabled) {
+        return;
+    }
+
+    compass.accumulate();
+
+    // 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;
+        }
+    }
+}
+
 void Rover::init_barometer(bool full_calibration)
 {
     gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");