From a07920c5b2696e28e24a18cd882ec5b563d8e6c9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 7 Jun 2017 10:28:34 +0900
Subject: [PATCH] Rover: add init_compass method

no functional change
---
 APMrover2/Rover.h     |  1 +
 APMrover2/sensors.cpp | 15 +++++++++++++++
 APMrover2/system.cpp  | 11 ++---------
 3 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 198e354218..2a2241da9b 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -415,6 +415,7 @@ private:
     void update_trigger(void);
     void update_alt();
     void gcs_failsafe_check(void);
+    void init_compass(void);
     void compass_accumulate(void);
     void compass_cal_update(void);
     void update_compass(void);
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index c588977914..67625189f7 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -1,5 +1,20 @@
 #include "Rover.h"
 
+// initialise compass
+void Rover::init_compass()
+{
+    if (!g.compass_enabled) {
+        return;
+    }
+
+    if (!compass.init()|| !compass.read()) {
+        cliSerial->printf("Compass initialisation failed!\n");
+        g.compass_enabled = false;
+    } else {
+        ahrs.set_compass(&compass);
+    }
+}
+
 /*
   if the compass is enabled then try to accumulate a reading
   also update initial location used for declination
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index 11897bb2ba..4b44cde790 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -145,15 +145,8 @@ void Rover::init_ardupilot()
     log_init();
 #endif
 
-    if (g.compass_enabled == true) {
-        if (!compass.init()|| !compass.read()) {
-            cliSerial->printf("Compass initialisation failed!\n");
-            g.compass_enabled = false;
-        } else {
-            ahrs.set_compass(&compass);
-            // compass.get_offsets();  // load offsets to account for airframe magnetic interference
-        }
-    }
+    // initialise compass
+    init_compass();
 
     // initialise sonar
     init_sonar();