Surface tracking in ArduSub (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 @@ class Parameters {
         // 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 @@ class Parameters {
         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 @@ class Parameters {
     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 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
         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
-    } rangefinder_state = { false, false, 0, 0 };
+        float inertial_alt_cm;                  // inertial alt at time of last rangefinder sample
+        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 @@ 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;
@@ -391,7 +395,6 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
 
 public:
     void mainloop_failsafe_check();
+    bool rangefinder_alt_ok() const WARN_IF_UNUSED;
 
     static Sub *_singleton;
 
@@ -611,6 +612,11 @@ class Sub : public AP_Vehicle {
 
     // 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
-# define RANGEFINDER_TILT_CORRECTION ENABLED
+#ifndef RANGEFINDER_TILT_CORRECTION        // by disable tilt correction for use of range finder data by EKF
+# 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 @@ 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
+        SURFTRAK =     21   // Track distance above seafloor (hold range)
     };
 
     // constructor
@@ -266,11 +267,45 @@ 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 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();
-
 }