ArduSub: make AP_RANGEFINDER_ENABLED remove more code

ArduSub/GCS_Sub.cpp

@@ -73,7 +73,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
     }
 #endif
 
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     const RangeFinder *rangefinder = RangeFinder::get_singleton();
     if (sub.rangefinder_state.enabled) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

ArduSub/Parameters.cpp

@@ -628,7 +628,7 @@ const AP_Param::Info Sub::var_info[] = {
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECT(mission, "MIS_",       AP_Mission),
 
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
     GOBJECT(rangefinder,   "RNGFND", RangeFinder),

ArduSub/Parameters.h

@@ -243,7 +243,7 @@ public:
 
     AP_Float        throttle_filt;
 
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     AP_Int8         rangefinder_signal_min;     // minimum signal quality for good rangefinder readings
     AP_Float        surftrak_depth;             // surftrak will try to keep sub below this depth
 #endif

ArduSub/Sub.h

@@ -625,10 +625,10 @@ public:
     // For Lua scripting, so index is 1..4, not 0..3
     uint8_t get_and_clear_button_count(uint8_t index);
 
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); }
     bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); }
-#endif // RANGEFINDER_ENABLED
+#endif // AP_RANGEFINDER_ENABLED
 #endif // AP_SCRIPTING_ENABLED
 };
 

ArduSub/config.h

@@ -46,10 +46,6 @@
 // Rangefinder
 //
 
-#ifndef RANGEFINDER_ENABLED
-# define RANGEFINDER_ENABLED ENABLED
-#endif
-
 #ifndef RANGEFINDER_HEALTH_MAX
 # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
 #endif

ArduSub/joystick.cpp

@@ -190,7 +190,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
     case JSButton::button_function_t::k_mode_poshold:
         set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
         break;
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     case JSButton::button_function_t::k_mode_surftrak:
         set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND);
         break;

ArduSub/mode_auto.cpp

@@ -426,6 +426,7 @@ void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
 // Return true if it is possible to recover from a rangefinder failure
 bool ModeAuto::auto_terrain_recover_start()
 {
+#if AP_RANGEFINDER_ENABLED
     // Check rangefinder status to see if recovery is possible
     switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
 
@@ -462,6 +463,9 @@ bool ModeAuto::auto_terrain_recover_start()
 
     gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
     return true;
+#else
+    return false;
+#endif
 }
 
 // Attempt recovery from terrain failsafe
@@ -470,7 +474,6 @@ bool ModeAuto::auto_terrain_recover_start()
 void ModeAuto::auto_terrain_recover_run()
 {
     float target_climb_rate = 0;
-    static uint32_t rangefinder_recovery_ms = 0;
 
     // if not armed set throttle to zero and exit immediately
     if (!motors.armed()) {
@@ -483,6 +486,8 @@ void ModeAuto::auto_terrain_recover_run()
         return;
     }
 
+#if AP_RANGEFINDER_ENABLED
+    static uint32_t rangefinder_recovery_ms = 0;
     switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
 
     case RangeFinder::Status::OutOfRangeLow:
@@ -529,6 +534,10 @@ void ModeAuto::auto_terrain_recover_run()
         rangefinder_recovery_ms = 0;
         return;
     }
+#else
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
+    sub.failsafe_terrain_act();
+#endif
 
     // exit on failure (timeout)
     if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {

ArduSub/mode_surftrak.cpp

@@ -39,8 +39,10 @@ bool ModeSurftrak::init(bool ignore_checks)
 
     if (!sub.rangefinder_alt_ok()) {
         sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading");
+#if AP_RANGEFINDER_ENABLED
     } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to hold range", sub.g.surftrak_depth * 0.01f);
+#endif
     }
 
     return true;
@@ -60,6 +62,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
 {
     bool success = false;
 
+#if AP_RANGEFINDER_ENABLED
     if (sub.control_mode != Number::SURFTRAK) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set");
     } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
@@ -84,6 +87,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
     } else {
         reset();
     }
+#endif
 
     return success;
 }
@@ -141,6 +145,7 @@ void ModeSurftrak::control_range() {
  */
 void ModeSurftrak::update_surface_offset()
 {
+#if AP_RANGEFINDER_ENABLED
     if (sub.rangefinder_alt_ok()) {
         // Get the latest terrain offset
         float rangefinder_terrain_offset_cm = sub.rangefinder_state.rangefinder_terrain_offset_cm;
@@ -162,4 +167,5 @@ void ModeSurftrak::update_surface_offset()
             sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
         }
     }
+#endif  // AP_RANGEFINDER_ENABLED
 }

ArduSub/sensors.cpp

@@ -17,7 +17,7 @@ void Sub::read_barometer()
 
 void Sub::init_rangefinder()
 {
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
     rangefinder.init(ROTATION_PITCH_270);
     rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
@@ -28,7 +28,7 @@ void Sub::init_rangefinder()
 // return rangefinder altitude in centimeters
 void Sub::read_rangefinder()
 {
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     rangefinder.update();
 
     // signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a

ArduSub/system.cpp

@@ -129,7 +129,7 @@ void Sub::init_ardupilot()
     last_pilot_heading = ahrs.yaw_sensor;
 
     // initialise rangefinder
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     init_rangefinder();
 #endif
 

ArduSub/terrain.cpp

@@ -8,7 +8,7 @@ void Sub::terrain_update()
 
     // tell the rangefinder our height, so it can go into power saving
     // mode if available
-#if RANGEFINDER_ENABLED == ENABLED
+#if AP_RANGEFINDER_ENABLED
     float height;
     if (terrain.height_above_terrain(height, true)) {
         rangefinder.set_estimated_terrain_height(height);