diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index c6615b8715..ef6281d4dd 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -93,16 +93,6 @@ void Sub::setup()
     fast_loopTimer = AP_HAL::micros();
 }
 
-/*
-  if the compass is enabled then try to accumulate a reading
- */
-void Sub::compass_accumulate(void)
-{
-    if (g.compass_enabled) {
-        compass.accumulate();
-    }
-}
-
 /*
   try to accumulate a baro reading
  */
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index c0edc51e4f..c2f76dc2e4 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -234,6 +234,7 @@ private:
             uint8_t at_bottom           : 1; // true if we are at the bottom
             uint8_t at_surface          : 1; // true if we are at the surface
             uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot
+            uint8_t compass_init_location:1; // true when the compass's initial location has been set
         };
         uint32_t value;
     } ap;
diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp
index eaf40ce4f3..e6ba8346d6 100644
--- a/ArduSub/commands.cpp
+++ b/ArduSub/commands.cpp
@@ -68,10 +68,6 @@ bool Sub::set_home(const Location& loc, bool lock)
 
     // init inav and compass declination
     if (ap.home_state == HOME_UNSET) {
-        // Set compass declination automatically
-        if (g.compass_enabled) {
-            compass.set_initial_location(gps.location().lat, gps.location().lng);
-        }
         // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
         scaleLongDown = longitude_scale(loc);
         // record home is set
diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
index 19e96543af..095b145b64 100644
--- a/ArduSub/sensors.cpp
+++ b/ArduSub/sensors.cpp
@@ -102,6 +102,28 @@ void Sub::init_compass()
     ahrs.set_compass(&compass);
 }
 
+/*
+  if the compass is enabled then try to accumulate a reading
+  also update initial location used for declination
+ */
+void Sub::compass_accumulate(void)
+{
+    if (!g.compass_enabled) {
+        return;
+    }
+
+    compass.accumulate();
+
+    // update initial location used for declination
+    if (!ap.compass_init_location) {
+        Location loc;
+        if (ahrs.get_position(loc)) {
+            compass.set_initial_location(loc.lat, loc.lng);
+            ap.compass_init_location = true;
+        }
+    }
+}
+
 // initialise optical flow sensor
 #if OPTFLOW == ENABLED
 void Sub::init_optflow()