Sub: add SURFTRAK mode

ArduSub/Attitude.cpp

@@ -123,46 +123,6 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
     return desired_rate;
 }
 
-// get_surface_tracking_climb_rate - hold vehicle at the desired distance above the ground
-//      returns climb rate (in cm/s) which should be passed to the position controller
-float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
-{
-#if RANGEFINDER_ENABLED == ENABLED
-    static uint32_t last_call_ms = 0;
-    float distance_error;
-    float velocity_correction;
-    float current_alt = inertial_nav.get_position_z_up_cm();
-
-    uint32_t now = AP_HAL::millis();
-
-    // reset target altitude if this controller has just been engaged
-    if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
-        target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
-    }
-    last_call_ms = now;
-
-    // adjust rangefinder target alt if motors have not hit their limits
-    if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
-        target_rangefinder_alt += target_rate * dt;
-    }
-
-    // do not let target altitude get too far from current altitude above ground
-    target_rangefinder_alt = constrain_float(target_rangefinder_alt,
-        rangefinder_state.alt_cm - pos_control.get_pos_error_z_down_cm(),
-        rangefinder_state.alt_cm + pos_control.get_pos_error_z_up_cm());
-
-    // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
-    distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
-    velocity_correction = distance_error * g.rangefinder_gain;
-    velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
-
-    // return combined pilot climb rate + rate to correct rangefinder alt error
-    return (target_rate + velocity_correction);
-#else
-    return (float)target_rate;
-#endif
-}
-
 // rotate vector from vehicle's perspective to North-East frame
 void Sub::rotate_body_frame_to_NE(float &x, float &y)
 {

ArduSub/GCS_Mavlink.cpp

@@ -144,6 +144,9 @@ bool GCS_MAVLINK_Sub::send_info()
     CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
     send_named_float("RollPitch", sub.roll_pitch_flag);
 
+    CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
+    send_named_float("RFTarget", sub.mode_surftrak.get_rangefinder_target_cm() / 100.0f);
+
     return true;
 }
 

ArduSub/Log.cpp

@@ -28,7 +28,13 @@ void Sub::Log_Write_Control_Tuning()
     // get terrain altitude
     float terr_alt = 0.0f;
 #if AP_TERRAIN_AVAILABLE
-    terrain.height_above_terrain(terr_alt, true);
+    if (terrain.enabled()) {
+        terrain.height_above_terrain(terr_alt, true);
+    } else {
+        terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;
+    }
+#else
+    terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;
 #endif
 
     struct log_Control_Tuning pkt = {
@@ -41,7 +47,7 @@ void Sub::Log_Write_Control_Tuning()
         desired_alt         : pos_control.get_pos_target_z_cm() / 100.0f,
         inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
         baro_alt            : barometer.get_altitude(),
-        desired_rangefinder_alt   : (int16_t)target_rangefinder_alt,
+        desired_rangefinder_alt   : (int16_t)mode_surftrak.get_rangefinder_target_cm(),
         rangefinder_alt           : rangefinder_state.alt_cm,
         terr_alt            : terr_alt,
         target_climb_rate   : (int16_t)pos_control.get_vel_target_z_cms(),

ArduSub/Parameters.cpp

@@ -75,16 +75,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Bitmask: 0:Roll,1:Pitch,2:Yaw
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
-#if RANGEFINDER_ENABLED == ENABLED
-    // @Param: RNGFND_GAIN
-    // @DisplayName: Rangefinder gain
-    // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the sub
-    // @Range: 0.01 2.0
-    // @Increment: 0.01
-    // @User: Standard
-    GSCALAR(rangefinder_gain,     "RNGFND_GAIN",           RANGEFINDER_GAIN_DEFAULT),
-#endif
-
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
     // @Description: Controls what action to take when GCS heartbeat is lost.
@@ -647,6 +637,20 @@ const AP_Param::Info Sub::var_info[] = {
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
     GOBJECT(rangefinder,   "RNGFND", RangeFinder),
+
+    // @Param: RNGFND_SQ_MIN
+    // @DisplayName: Rangefinder signal quality minimum
+    // @Description: Minimum signal quality for good rangefinder readings
+    // @Range: 0 100
+    // @User: Advanced
+    GSCALAR(rangefinder_signal_min, "RNGFND_SQ_MIN", RANGEFINDER_SIGNAL_MIN_DEFAULT),
+
+    // @Param: SURFTRAK_DEPTH
+    // @DisplayName: SURFTRAK minimum depth
+    // @Description: Minimum depth to engage SURFTRAK mode
+    // @Units: cm
+    // @User: Standard
+    GSCALAR(surftrak_depth, "SURFTRAK_DEPTH", SURFTRAK_DEPTH_DEFAULT),
 #endif
 
 #if AP_TERRAIN_AVAILABLE

ArduSub/Parameters.h

@@ -190,7 +190,7 @@ public:
         // Misc Sub settings
         k_param_log_bitmask = 165,
         k_param_angle_max = 167,
-        k_param_rangefinder_gain,
+        k_param_rangefinder_gain, // deprecated
         k_param_wp_yaw_behavior = 170,
         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
@@ -228,6 +228,8 @@ public:
         k_param_cam_slew_limit = 237, // deprecated
         k_param_lights_steps,
         k_param_pilot_speed_dn,
+        k_param_rangefinder_signal_min,
+        k_param_surftrak_depth,
 
         k_param_vehicle = 257, // vehicle common block of parameters
     };
@@ -242,7 +244,8 @@ public:
     AP_Float        throttle_filt;
 
 #if RANGEFINDER_ENABLED == ENABLED
-    AP_Float        rangefinder_gain;
+    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
 
     AP_Int8         failsafe_leak;              // leak detection failsafe behavior

ArduSub/Sub.h

@@ -111,6 +111,7 @@ public:
     friend class ModeStabilize;
     friend class ModeAcro;
     friend class ModeAlthold;
+    friend class ModeSurftrak;
     friend class ModeGuided;
     friend class ModePoshold;
     friend class ModeAuto;
@@ -147,9 +148,13 @@ private:
         bool enabled:1;
         bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
+        int16_t min_cm;     // min rangefinder distance (in cm)
+        int16_t max_cm;     // max rangefinder distance (in cm)
         uint32_t last_healthy_ms;
-        LowPassFilterFloat alt_cm_filt; // altitude filter
+        float inertial_alt_cm;                  // inertial alt at time of last rangefinder sample
-    } rangefinder_state = { false, false, 0, 0 };
+        float rangefinder_terrain_offset_cm;    // terrain height above EKF origin
+        LowPassFilterFloat alt_cm_filt;         // altitude filter
+    } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 };
 
 #if AP_RPM_ENABLED
     AP_RPM rpm_sensor;
@@ -268,7 +273,6 @@ private:
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;
-    float target_rangefinder_alt;      // desired altitude in cm above the ground
 
     // Turn counter
     int32_t quarter_turn_count;
@@ -391,7 +395,6 @@ private:
     float get_roi_yaw();
     float get_look_ahead_yaw();
     float get_pilot_desired_climb_rate(float throttle_control);
-    float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
     void rotate_body_frame_to_NE(float &x, float &y);
 #if HAL_LOGGING_ENABLED
     // methods for AP_Vehicle:
@@ -475,7 +478,6 @@ private:
     void read_barometer(void);
     void init_rangefinder(void);
     void read_rangefinder(void);
-    bool rangefinder_alt_ok(void) const;
     void terrain_update();
     void terrain_logging();
     void init_ardupilot() override;
@@ -533,9 +535,6 @@ private:
     void translate_circle_nav_rp(float &lateral_out, float &forward_out);
     void translate_pos_control_rp(float &lateral_out, float &forward_out);
 
-    bool surface_init(void);
-    void surface_run();
-
     void stats_update();
 
     uint16_t get_pilot_speed_dn() const;
@@ -587,6 +586,7 @@ private:
     ModeCircle mode_circle;
     ModeSurface mode_surface;
     ModeMotordetect mode_motordetect;
+    ModeSurftrak mode_surftrak;
 
     // Auto
     AutoSubMode auto_mode;   // controls which auto controller is run
@@ -598,6 +598,7 @@ private:
 
 public:
     void mainloop_failsafe_check();
+    bool rangefinder_alt_ok() const WARN_IF_UNUSED;
 
     static Sub *_singleton;
 
@@ -611,6 +612,11 @@ 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
+    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_SCRIPTING_ENABLED
 };
 

ArduSub/config.h

@@ -54,14 +54,6 @@
 # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
 #endif
 
-#ifndef RANGEFINDER_GAIN_DEFAULT
-# define RANGEFINDER_GAIN_DEFAULT 0.8f     // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
-#endif
-
-#ifndef THR_SURFACE_TRACKING_VELZ_MAX
-# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
-#endif
-
 #ifndef RANGEFINDER_TIMEOUT_MS
 # define RANGEFINDER_TIMEOUT_MS  1000      // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
@@ -70,8 +62,16 @@
 # define RANGEFINDER_WPNAV_FILT_HZ   0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
 #endif
 
-#ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
+#ifndef RANGEFINDER_TILT_CORRECTION        // by disable tilt correction for use of range finder data by EKF
-# define RANGEFINDER_TILT_CORRECTION ENABLED
+# define RANGEFINDER_TILT_CORRECTION DISABLED
+#endif
+
+#ifndef RANGEFINDER_SIGNAL_MIN_DEFAULT
+# define RANGEFINDER_SIGNAL_MIN_DEFAULT 90 // rangefinder readings with signal quality below this value are ignored
+#endif
+
+#ifndef SURFTRAK_DEPTH_DEFAULT
+# define SURFTRAK_DEPTH_DEFAULT -50.0f     // surftrak will try to keep the sub below this depth
 #endif
 
 // Avoidance (relies on Proximity and Fence)

ArduSub/joystick.cpp

@@ -190,6 +190,11 @@ 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
+    case JSButton::button_function_t::k_mode_surftrak:
+        set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND);
+        break;
+#endif
 
     case JSButton::button_function_t::k_mount_center:
 #if HAL_MOUNT_ENABLED

ArduSub/mode.cpp

@@ -38,6 +38,9 @@ Mode *Sub::mode_from_mode_num(const Mode::Number mode)
     case Mode::Number::ALT_HOLD:
         ret = &mode_althold;
         break;
+    case Mode::Number::SURFTRAK:
+        ret = &mode_surftrak;
+        break;
     case Mode::Number::POSHOLD:
         ret = &mode_poshold;
         break;

ArduSub/mode.h

@@ -49,7 +49,8 @@ public:
         SURFACE =       9,  // automatically return to surface, pilot maintains horizontal control
         POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
         MANUAL =       19,  // Pass-through input with no stabilization
-        MOTOR_DETECT = 20   // Automatically detect motors orientation
+        MOTOR_DETECT = 20,  // Automatically detect motors orientation
+        SURFTRAK =     21   // Track distance above seafloor (hold range)
     };
 
     // constructor
@@ -266,11 +267,45 @@ public:
 
 protected:
 
+    void run_pre();
+    void run_post();
+
     const char *name() const override { return "ALT_HOLD"; }
     const char *name4() const override { return "ALTH"; }
 };
 
 
+class ModeSurftrak : public ModeAlthold
+{
+
+public:
+    // constructor
+    ModeSurftrak();
+
+    void run() override;
+
+    bool init(bool ignore_checks) override;
+
+    float get_rangefinder_target_cm() const WARN_IF_UNUSED { return rangefinder_target_cm; }
+    bool set_rangefinder_target_cm(float target_cm);
+
+protected:
+
+    const char *name() const override { return "SURFTRAK"; }
+    const char *name4() const override { return "STRK"; }
+
+private:
+
+    void reset();
+    void control_range();
+    void update_surface_offset();
+
+    float rangefinder_target_cm;
+
+    bool pilot_in_control;            // pilot is moving up/down
+    float pilot_control_start_z_cm;   // alt when pilot took control
+};
+
 class ModeGuided : public Mode
 {
 

ArduSub/mode_althold.cpp

@@ -22,6 +22,13 @@ bool ModeAlthold::init(bool ignore_checks) {
 // althold_run - runs the althold controller
 // should be called at 100hz or more
 void ModeAlthold::run()
+{
+    run_pre();
+    control_depth();
+    run_post();
+}
+
+void ModeAlthold::run_pre()
 {
     uint32_t tnow = AP_HAL::millis();
 
@@ -84,13 +91,14 @@ void ModeAlthold::run()
             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
             sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
 
-        } else { // call attitude controller holding absolute absolute bearing
+        } else { // call attitude controller holding absolute bearing
             attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true);
         }
     }
+}
 
-    control_depth();
+void ModeAlthold::run_post()
-
+{
     motors.set_forward(channel_forward->norm_input());
     motors.set_lateral(channel_lateral->norm_input());
 }
@@ -111,5 +119,4 @@ void ModeAlthold::control_depth() {
 
     position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
     position_control->update_z_controller();
-
 }

ArduSub/mode_surftrak.cpp

@@ -0,0 +1,165 @@
+#include "Sub.h"
+
+/*
+ * SURFTRAK (surface tracking) -- a variation on ALT_HOLD (depth hold)
+ *
+ * SURFTRAK starts in the "reset" state (rangefinder_target_cm < 0). SURFTRAK exits the reset state when these
+ * conditions are met:
+ * -- There is a good rangefinder reading (the rangefinder is healthy, the reading is between min and max, etc.)
+ * -- The sub is below SURFTRAK_DEPTH
+ *
+ * During normal operation, SURFTRAK sets the offset target to the current terrain altitude estimate and calls
+ * AC_PosControl to do the rest.
+ *
+ * We generally do not want to reset SURFTRAK if the rangefinder glitches, since that will result in a new rangefinder
+ * target. E.g., if a pilot is running 1m above the seafloor, there is a glitch, and the next rangefinder reading shows
+ * 1.1m, the desired behavior is to move 10cm closer to the seafloor, vs setting a new target of 1.1m above the
+ * seafloor.
+ *
+ * If the pilot takes control, SURFTRAK uses the change in depth readings to adjust the rangefinder target. This
+ * minimizes the "bounce back" that can happen as the slower rangefinder catches up to the quicker barometer.
+ */
+
+#define INVALID_TARGET (-1)
+#define HAS_VALID_TARGET (rangefinder_target_cm > 0)
+
+ModeSurftrak::ModeSurftrak() :
+        rangefinder_target_cm(INVALID_TARGET),
+        pilot_in_control(false),
+        pilot_control_start_z_cm(0)
+{ }
+
+bool ModeSurftrak::init(bool ignore_checks)
+{
+    if (!ModeAlthold::init(ignore_checks)) {
+        return false;
+    }
+
+    reset();
+
+    if (!sub.rangefinder_alt_ok()) {
+        sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading");
+    } 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);
+    }
+
+    return true;
+}
+
+void ModeSurftrak::run()
+{
+    run_pre();
+    control_range();
+    run_post();
+}
+
+/*
+ * Set the rangefinder target, return true if successful. This may be called from scripting so run a few extra checks.
+ */
+bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
+{
+    bool success = false;
+
+    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) {
+        sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f);
+    } else if (target_cm < (float)sub.rangefinder_state.min_cm) {
+        sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored");
+    } else if (target_cm > (float)sub.rangefinder_state.max_cm) {
+        sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored");
+    } else {
+        success = true;
+    }
+
+    if (success) {
+        rangefinder_target_cm = target_cm;
+        sub.gcs().send_text(MAV_SEVERITY_INFO, "rangefinder target is %.2f meters", rangefinder_target_cm * 0.01f);
+
+        // Initialize the terrain offset
+        auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
+        sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm);
+        sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm);
+
+    } else {
+        reset();
+    }
+
+    return success;
+}
+
+void ModeSurftrak::reset()
+{
+    rangefinder_target_cm = INVALID_TARGET;
+
+    // Reset the terrain offset
+    sub.pos_control.set_pos_offset_z_cm(0);
+    sub.pos_control.set_pos_offset_target_z_cm(0);
+}
+
+/*
+ * Main controller, call at 100hz+
+ */
+void ModeSurftrak::control_range() {
+    float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+    target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up);
+
+    // Desired_climb_rate returns 0 when within the deadzone
+    if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
+        if (pilot_in_control) {
+            // Pilot has released control; apply the delta to the rangefinder target
+            set_rangefinder_target_cm(rangefinder_target_cm + inertial_nav.get_position_z_up_cm() - pilot_control_start_z_cm);
+            pilot_in_control = false;
+        }
+        if (sub.ap.at_surface) {
+            // Set target depth to 5 cm below SURFACE_DEPTH and reset
+            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f));
+            reset();
+        } else if (sub.ap.at_bottom) {
+            // Set target depth to 10 cm above bottom and reset
+            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm()));
+            reset();
+        } else {
+            // Typical operation
+            update_surface_offset();
+        }
+    } else if (HAS_VALID_TARGET && !pilot_in_control) {
+        // Pilot has taken control; note the current depth
+        pilot_control_start_z_cm = inertial_nav.get_position_z_up_cm();
+        pilot_in_control = true;
+    }
+
+    // Set the target altitude from the climb rate and the terrain offset
+    position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
+
+    // Run the PID controllers
+    position_control->update_z_controller();
+}
+
+/*
+ * Update the AC_PosControl terrain offset if we have a good rangefinder reading
+ */
+void ModeSurftrak::update_surface_offset()
+{
+    if (sub.rangefinder_alt_ok()) {
+        // Get the latest terrain offset
+        float rangefinder_terrain_offset_cm = sub.rangefinder_state.rangefinder_terrain_offset_cm;
+
+        // Handle the first reading or a reset
+        if (!HAS_VALID_TARGET && sub.rangefinder_state.inertial_alt_cm < sub.g.surftrak_depth) {
+            set_rangefinder_target_cm(sub.rangefinder_state.inertial_alt_cm - rangefinder_terrain_offset_cm);
+        }
+
+        if (HAS_VALID_TARGET) {
+            // Will the new offset target cause the sub to ascend above SURFTRAK_DEPTH?
+            float desired_z_cm = rangefinder_terrain_offset_cm + rangefinder_target_cm;
+            if (desired_z_cm >= sub.g.surftrak_depth) {
+                // Adjust the terrain offset to stay below SURFTRAK_DEPTH, this should avoid "at_surface" events
+                rangefinder_terrain_offset_cm += sub.g.surftrak_depth - desired_z_cm;
+            }
+
+            // Set the offset target, AC_PosControl will do the rest
+            sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
+        }
+    }
+}

ArduSub/sensors.cpp

@@ -18,6 +18,7 @@ void Sub::read_barometer()
 void Sub::init_rangefinder()
 {
 #if RANGEFINDER_ENABLED == 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);
     rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
@@ -30,7 +31,13 @@ void Sub::read_rangefinder()
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-    rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
+    // signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a
+    int8_t signal_quality_pct = rangefinder.signal_quality_pct_orient(ROTATION_PITCH_270);
+
+    rangefinder_state.alt_healthy =
+            (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) &&
+            (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) &&
+            (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min);
 
     int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
 
@@ -40,11 +47,13 @@ void Sub::read_rangefinder()
 #endif
 
     rangefinder_state.alt_cm = temp_alt;
+    rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
+    rangefinder_state.min_cm = rangefinder.min_distance_cm_orient(ROTATION_PITCH_270);
+    rangefinder_state.max_cm = rangefinder.max_distance_cm_orient(ROTATION_PITCH_270);
 
-    // filter rangefinder for use by AC_WPNav
+    // calculate rangefinder_terrain_offset_cm
-    uint32_t now = AP_HAL::millis();
-
     if (rangefinder_state.alt_healthy) {
+        uint32_t now = AP_HAL::millis();
         if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
             // reset filter if we haven't used it within the last second
             rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
@@ -52,22 +61,31 @@ void Sub::read_rangefinder()
             rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f);
         }
         rangefinder_state.last_healthy_ms = now;
+        rangefinder_state.rangefinder_terrain_offset_cm =
+                sub.rangefinder_state.inertial_alt_cm - sub.rangefinder_state.alt_cm_filt.get();
     }
 
     // send rangefinder altitude and health to waypoint navigation library
-    const float terrain_offset_cm = inertial_nav.get_position_z_up_cm() - rangefinder_state.alt_cm_filt.get();
+    wp_nav.set_rangefinder_terrain_offset(
-    wp_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, terrain_offset_cm);
+            rangefinder_state.enabled,
-    circle_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav.rangefinder_used(), rangefinder_state.alt_healthy, terrain_offset_cm);
+            rangefinder_state.alt_healthy,
-
+            rangefinder_state.rangefinder_terrain_offset_cm);
+    circle_nav.set_rangefinder_terrain_offset(
+            rangefinder_state.enabled && wp_nav.rangefinder_used(),
+            rangefinder_state.alt_healthy,
+            rangefinder_state.rangefinder_terrain_offset_cm);
 #else
     rangefinder_state.enabled = false;
     rangefinder_state.alt_healthy = false;
     rangefinder_state.alt_cm = 0;
+    rangefinder_state.inertial_alt_cm = 0;
+    rangefinder_state.rangefinder_terrain_offset_cm = 0;
 #endif
 }
 
 // return true if rangefinder_alt can be used
 bool Sub::rangefinder_alt_ok() const
 {
-    return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
+    uint32_t now = AP_HAL::millis();
+    return (rangefinder_state.enabled && rangefinder_state.alt_healthy && now - rangefinder_state.last_healthy_ms < RANGEFINDER_TIMEOUT_MS);
 }

Tools/autotest/ardusub.py

@@ -14,6 +14,7 @@ from pymavlink import mavutil
 import vehicle_test_suite
 from vehicle_test_suite import NotAchievedException
 from vehicle_test_suite import AutoTestTimeoutException
+from vehicle_test_suite import PreconditionFailedException
 
 if sys.version_info[0] < 3:
     ConnectionResetError = AutoTestTimeoutException
@@ -114,7 +115,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
             raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
         pwm = 1300
         if msg.relative_alt/1000.0 < -6.0:
-            # need to g`o up, not down!
+            # need to go up, not down!
             pwm = 1700
         self.set_rc(Joystick.Throttle, pwm)
         self.wait_altitude(altitude_min=-6, altitude_max=-5)
@@ -204,8 +205,69 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
         if ex:
             raise ex
 
+    def watch_distance_maintained(self, delta=0.3, timeout=5.0):
+        """Watch and wait for the rangefinder reading to be maintained"""
+        tstart = self.get_sim_time_cached()
+        previous_distance = self.mav.recv_match(type='RANGEFINDER', blocking=True).distance
+        self.progress('Distance to be watched: %.2f' % previous_distance)
+        while True:
+            m = self.mav.recv_match(type='RANGEFINDER', blocking=True)
+            if self.get_sim_time_cached() - tstart > timeout:
+                self.progress('Distance hold done: %f' % previous_distance)
+                return
+            if abs(m.distance - previous_distance) > delta:
+                raise NotAchievedException(
+                    "Distance not maintained: want %.2f (+/- %.2f) got=%.2f" %
+                    (previous_distance, delta, m.distance))
+
+    def Surftrak(self):
+        """Test SURFTRAK mode"""
+
+        if self.get_parameter('RNGFND1_MAX_CM') != 3000.0:
+            raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0)
+
+        # Something closer to Bar30 noise
+        self.context_push()
+        self.set_parameter("SIM_BARO_RND", 0.01)
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode('MANUAL')
+
+        # Dive to -5m, outside of rangefinder range, will act like ALT_HOLD
+        pwm = 1300 if self.get_altitude(relative=True) > -6 else 1700
+        self.set_rc(Joystick.Throttle, pwm)
+        self.wait_altitude(altitude_min=-6, altitude_max=-5, relative=False, timeout=60)
+        self.set_rc(Joystick.Throttle, 1500)
+        self.delay_sim_time(1)
+        self.context_collect('STATUSTEXT')
+        self.change_mode(21)
+        self.wait_statustext('waiting for a rangefinder reading', check_context=True)
+        self.context_clear_collection('STATUSTEXT')
+        self.watch_altitude_maintained()
+
+        # Move into range, should set a rangefinder target and maintain it
+        self.set_rc(Joystick.Throttle, 1300)
+        self.wait_altitude(altitude_min=-26, altitude_max=-25, relative=False, timeout=60)
+        self.set_rc(Joystick.Throttle, 1500)
+        self.delay_sim_time(4)
+        self.wait_statustext('rangefinder target is', check_context=True)
+        self.context_clear_collection('STATUSTEXT')
+        self.watch_distance_maintained()
+
+        # Move a few meters, should apply a delta and maintain the new rangefinder target
+        self.set_rc(Joystick.Throttle, 1300)
+        self.wait_altitude(altitude_min=-31, altitude_max=-30, relative=False, timeout=60)
+        self.set_rc(Joystick.Throttle, 1500)
+        self.delay_sim_time(4)
+        self.wait_statustext('rangefinder target is', check_context=True)
+        self.watch_distance_maintained()
+
+        self.disarm_vehicle()
+        self.context_pop()
+
     def ModeChanges(self, delta=0.2):
-        """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude"""
+        """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and SURFTRAK (mode 21) affects altitude"""
         self.wait_ready_to_arm()
         self.arm_vehicle()
         # zero buoyancy and no baro noise
@@ -225,12 +287,16 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
         self.delay_sim_time(2)
         self.change_mode('STABILIZE')
         self.delay_sim_time(2)
+        self.change_mode(21)
+        self.delay_sim_time(2)
         self.change_mode('ALT_HOLD')
         self.delay_sim_time(2)
         self.change_mode('STABILIZE')
         self.delay_sim_time(2)
         self.change_mode('ALT_HOLD')
         self.delay_sim_time(2)
+        self.change_mode(21)
+        self.delay_sim_time(2)
         self.change_mode('MANUAL')
         self.disarm_vehicle()
         final_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
@@ -562,6 +628,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
         ret.extend([
             self.DiveManual,
             self.AltitudeHold,
+            self.Surftrak,
             self.RngfndQuality,
             self.PositionHold,
             self.ModeChanges,

Tools/autotest/default_params/sub.parm

@@ -86,3 +86,4 @@ BARO_EXT_BUS 1
 PILOT_ACCEL_Z 200
 PILOT_SPEED_UP 200
 PSC_JERK_Z 8
+LOG_BITMASK,180222