Add RNG_HOLD mode

ArduSub/Attitude.cpp

@@ -123,6 +123,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
     return desired_rate;
 }
 
+#if 0
 // 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)
@@ -162,6 +163,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
     return (float)target_rate;
 #endif
 }
+#endif
 
 // rotate vector from vehicle's perspective to North-East frame
 void Sub::rotate_body_frame_to_NE(float &x, float &y)

ArduSub/Log.cpp

@@ -41,7 +41,11 @@ 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,
+#if RANGEFINDER_ENABLED == ENABLED
+        desired_rangefinder_alt   : (int16_t)surface_tracking.get_target_rangefinder_cm(),
+#else
+        desired_rangefinder_alt   : 0,
+#endif
         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

@@ -583,6 +583,10 @@ const AP_Param::Info Sub::var_info[] = {
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
     GOBJECT(rangefinder,   "RNGFND", RangeFinder),
+
+    // @Group: RNGFND_PID_
+    // @Path: ../libraries/AC_PID/AC_PID.cpp
+    GOBJECTN(surface_tracking.pid_rangefinder, pid_rangefinder, "RNGFND_PID_", AC_PID),
 #endif
 
 #if AP_TERRAIN_AVAILABLE

ArduSub/Parameters.h

@@ -155,6 +155,7 @@ class Parameters {
         k_param_pi_vel_xy, // deprecated
         k_param_p_vel_z, // deprecated
         k_param_pid_accel_z, // deprecated
+        k_param_pid_rangefinder,
 
 
         // Failsafes

ArduSub/Sub.h

@@ -62,6 +62,7 @@
 #include <AP_JSButton/AP_JSButton.h>   // Joystick/gamepad button function assignment
 #include <AP_LeakDetector/AP_LeakDetector.h> // Leak detector
 #include <AP_Proximity/AP_Proximity.h>
+#include <AC_PID/AC_PID.h>
 
 // Local modules
 #include "defines.h"
@@ -114,6 +115,7 @@ class Sub : public AP_Vehicle {
     friend class ModeStabilize;
     friend class ModeAcro;
     friend class ModeAlthold;
+    friend class ModeRnghold;
     friend class ModeGuided;
     friend class ModePoshold;
     friend class ModeAuto;
@@ -156,6 +158,35 @@ class Sub : public AP_Vehicle {
         LowPassFilterFloat alt_cm_filt; // altitude filter
     } rangefinder_state = { false, false, 0, 0 };
 
+#if RANGEFINDER_ENABLED == ENABLED
+    class SurfaceTracking {
+    public:
+        // pilot can enable or disable tracking
+        void enable(bool _enabled);
+
+        // reset controller, target_rangefinder_cm = next healthy rangefinder reading
+        void reset();
+
+        // get target rangefinder
+        float get_target_rangefinder_cm() const { return target_rangefinder_cm; }
+
+        // set target rangefinder
+        void set_target_rangefinder_cm(float new_target_cm);
+
+        // track seafloor, call from main control loop
+        void update_surface_offset();
+
+        // rangefinder PID, must be public so that AP_Param can see it
+        AC_PID pid_rangefinder{RNGFND_P_DEFAULT, RNGFND_I_DEFAULT, RNGFND_D_DEFAULT,
+                               0.0, 0.0, 5.0, 5.0, 5.0};
+
+    private:
+        bool enabled = false;                 // true if pilot enabled surface tracking
+        bool reset_target = false;            // true if target should be reset
+        float target_rangefinder_cm = 0;      // target distance to seafloor
+    } surface_tracking;
+#endif
+
 #if AP_RPM_ENABLED
     AP_RPM rpm_sensor;
 #endif
@@ -273,7 +304,6 @@ class Sub : public AP_Vehicle {
     // 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;
@@ -566,6 +596,7 @@ class Sub : public AP_Vehicle {
     ModeStabilize mode_stabilize;
     ModeAcro mode_acro;
     ModeAlthold mode_althold;
+    ModeRnghold mode_rnghold;
     ModeAuto mode_auto;
     ModeGuided mode_guided;
     ModePoshold mode_poshold;

ArduSub/config.h

@@ -58,6 +58,18 @@
 # define RANGEFINDER_GAIN_DEFAULT 0.8f     // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
 #endif
 
+#ifndef RNGFND_P_DEFAULT
+# define RNGFND_P_DEFAULT 0.4f     // default P term for rangefinder_pid
+#endif
+
+#ifndef RNGFND_I_DEFAULT
+# define RNGFND_I_DEFAULT 0.0f     // default I term for rangefinder_pid
+#endif
+
+#ifndef RNGFND_D_DEFAULT
+# define RNGFND_D_DEFAULT 0.0f     // default D term for rangefinder_pid
+#endif
+
 #ifndef THR_SURFACE_TRACKING_VELZ_MAX
 # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
 #endif

ArduSub/joystick.cpp

@@ -181,6 +181,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_range_hold:
+        set_mode(Mode::Number::RNG_HOLD, 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::RNG_HOLD:
+        ret = &mode_rnghold;
+        break;
     case Mode::Number::POSHOLD:
         ret = &mode_poshold;
         break;

ArduSub/mode.h

@@ -49,7 +49,8 @@ class Mode
         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
+        RNG_HOLD =     21   // Hold rangefinder value (distance above terrain)
     };
 
     // constructor
@@ -266,11 +267,35 @@ class ModeAlthold : public Mode
 
 protected:
 
+    void run_pre();
+    void run_post();
+
     const char *name() const override { return "ALT_HOLD"; }
     const char *name4() const override { return "ALTH"; }
 };
 
 
+class ModeRnghold : public ModeAlthold
+{
+
+public:
+    // inherit constructor
+    using ModeAlthold::ModeAlthold;
+
+    virtual void run() override;
+
+    bool init(bool ignore_checks) override;
+
+protected:
+
+    const char *name() const override { return "RNG_HOLD"; }
+    const char *name4() const override { return "RNGH"; }
+
+private:
+
+    void control_range();
+};
+
 class ModeGuided : public Mode
 {
 

ArduSub/mode_althold.cpp

@@ -19,9 +19,14 @@ bool ModeAlthold::init(bool ignore_checks) {
     return true;
 }
 
-// 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();
 
@@ -87,9 +92,10 @@ void ModeAlthold::run()
             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());
 }
@@ -110,5 +116,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_rnghold.cpp

@@ -0,0 +1,73 @@
+#include "Sub.h"
+
+/*
+ * RNG_HOLD (rangefinder hold) -- a variation on ALT_HOLD (depth hold)
+ *
+ * The real work for ALT_HOLD and RNG_HOLD is handled by AC_PosControl, which provides 2 inputs for depth:
+ * -- target depth (sub.pos_control._pos_target.z). This is the desired depth, plus an offset.
+ * -- target offset (sub.pos_control._pos_offset_target_z). This is the desired offset.
+ *
+ * ALT_HOLD and RNG_HOLD set the target depth in these situations:
+ * -- During initialization, we call pos_control.init_z_controller(). This sets target depth to the current depth.
+ * -- If the sub hits the surface or bottom we call pos_control.set_pos_target_z_cm().
+ * -- If the pilot takes control we call pos_control.set_pos_target_z_from_climb_rate_cm().
+ *
+ * At the end of the control loop ALT_HOLD and RNG_HOLD call pos_control.update_z_controller() to pass the buck.
+ *
+ * ALT_HOLD does not use the target offset.
+ *
+ * RNG_HOLD sets the target offset to implement surface tracking. This is handled by Sub::SurfaceTracking. We call
+ * SurfaceTracking in these situations:
+ * -- During initialization, we call surface_tracking.enable().
+ * -- During normal operation, we call surface_tracking.update_surface_offset().
+ * -- If the sub hits the surface or bottom, or the pilot takes control, we call surface_tracking.reset().
+ */
+
+bool ModeRnghold::init(bool ignore_checks)
+{
+    if (!ModeAlthold::init(ignore_checks)) {
+        return false;
+    }
+
+    // enable surface tracking
+    sub.surface_tracking.enable(true);
+
+    return true;
+}
+
+void ModeRnghold::run()
+{
+    run_pre();
+    control_range();
+    run_post();
+}
+
+void ModeRnghold::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 (sub.ap.at_surface) {
+            // set target depth to 5 cm below SURFACE_DEPTH
+            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f));
+            sub.surface_tracking.reset();
+        } else if (sub.ap.at_bottom) {
+            // set target depth to 10 cm above bottom
+            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm()));
+            sub.surface_tracking.reset();
+        } else {
+            // normal operation
+            sub.surface_tracking.update_surface_offset();
+        }
+    } else {
+        // pilot is in control
+        sub.surface_tracking.reset();
+    }
+
+    // set the target z from the climb rate and the z offset, and adjust the z vel and accel targets
+    position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
+
+    // run the z vel and accel PID controllers
+    position_control->update_z_controller();
+}

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);
@@ -69,5 +70,6 @@ void Sub::read_rangefinder()
 // 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);
 }