Sub: 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

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

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;

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

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