Sub: move enabled parameter into compass library

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

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

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

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
 

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

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

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) {
+    AP::compass().init();
-        init_compass();
-    }
 
 #if OPTFLOW == ENABLED
     // make optflow available to AHRS