From b1c4a5ade7f63cc8bb51f5f51244f21004cf3094 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 24 Mar 2019 14:27:18 +1100
Subject: [PATCH] Sub: move enabled parameter into compass library

---
 ArduSub/ArduSub.cpp    |  2 +-
 ArduSub/GCS_Sub.cpp    |  4 ++--
 ArduSub/Parameters.cpp |  7 -------
 ArduSub/Parameters.h   |  4 +---
 ArduSub/Sub.h          |  1 -
 ArduSub/sensors.cpp    | 13 -------------
 ArduSub/system.cpp     |  4 +---
 7 files changed, 5 insertions(+), 30 deletions(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index fa8898cb3c..06dc00a9d2 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -167,7 +167,7 @@ void Sub::update_batt_compass()
     // read battery before compass because it may be used for motor interference compensation
     battery.read();
 
-    if (g.compass_enabled) {
+    if (AP::compass().enabled()) {
         // update compass with throttle value - used for compassmot
         compass.set_throttle(motors.get_throttle());
         compass.read();
diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp
index 0409deb071..3b48e02824 100644
--- a/ArduSub/GCS_Sub.cpp
+++ b/ArduSub/GCS_Sub.cpp
@@ -20,7 +20,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
         MAV_SYS_STATUS_SENSOR_YAW_POSITION;
 
     // first what sensors/controllers we have
-    if (sub.g.compass_enabled) {
+    if (AP::compass().enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
@@ -67,7 +67,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
     }
     AP_AHRS &ahrs = AP::ahrs();
     const Compass &compass = AP::compass();
-    if (sub.g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
+    if (AP::compass().enabled() && compass.healthy() && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
     if (gps.is_healthy()) {
diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index 321b56b553..86c47be31b 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -158,13 +158,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(xtrack_angle_limit,"XTRACK_ANG_LIM", 45),
 
-    // @Param: MAG_ENABLE
-    // @DisplayName: Compass enable/disable
-    // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass
-    // @Values: 0:Disabled,1:Enabled
-    // @User: Standard
-    GSCALAR(compass_enabled,        "MAG_ENABLE",   ENABLED),
-
     // @Param: WP_YAW_BEHAVIOR
     // @DisplayName: Yaw behaviour during missions
     // @Description: Determines how the autopilot controls the yaw during missions and RTL
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index d835e8423f..1f9154e63c 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -180,7 +180,7 @@ public:
         k_param_xtrack_angle_limit, // Angle limit for crosstrack correction in Auto modes (degrees)
         k_param_pilot_speed_up,     // renamed from k_param_pilot_velocity_z_max
         k_param_pilot_accel_z,
-        k_param_compass_enabled,
+        k_param_compass_enabled_deprecated,
         k_param_surface_depth,
         k_param_rc_speed, // Main output pwm frequency
         k_param_gcs_pid_mask = 178,
@@ -237,8 +237,6 @@ public:
 
     AP_Int8         xtrack_angle_limit;
 
-    AP_Int8         compass_enabled;
-
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
     AP_Int8         rc_feel_rp;                 // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
 
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 80d2d58bad..11404edbb3 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -607,7 +607,6 @@ private:
     void init_rangefinder(void);
     void read_rangefinder(void);
     bool rangefinder_alt_ok(void);
-    void init_compass();
 #if OPTFLOW == ENABLED
     void init_optflow();
 #endif
diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
index 35b7c567b5..1d9d5bb549 100644
--- a/ArduSub/sensors.cpp
+++ b/ArduSub/sensors.cpp
@@ -80,19 +80,6 @@ void Sub::rpm_update(void)
 }
 #endif
 
-// initialise compass
-void Sub::init_compass()
-{
-    compass.init();
-    if (!compass.read()) {
-        // make sure we don't pass a broken compass to DCM
-        hal.console->println("COMPASS INIT ERROR");
-        AP::logger().Write_Error(LogErrorSubsystem::COMPASS,LogErrorCode::FAILED_TO_INITIALISE);
-        return;
-    }
-    ahrs.set_compass(&compass);
-}
-
 /*
   initialise compass's location used for declination
  */
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 51d95d9cb9..77bbf31f5b 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -104,9 +104,7 @@ void Sub::init_ardupilot()
     gps.set_log_gps_bit(MASK_LOG_GPS);
     gps.init(serial_manager);
 
-    if (g.compass_enabled) {
-        init_compass();
-    }
+    AP::compass().init();
 
 #if OPTFLOW == ENABLED
     // make optflow available to AHRS