Sub: limit poshold xy velocity to PILOT_SPEED to avoid bounceback

ArduSub/Attitude.cpp

@@ -117,6 +117,33 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
     }
 }
 
+// behavior is similar to Sub::get_pilot_desired_climb_rate
+float Sub::get_pilot_desired_horizontal_rate(RC_Channel *channel) const
+{
+    if (failsafe.pilot_input) {
+        return 0;
+    }
+
+    // forward and lateral sticks have center trim, unlike throttle
+    auto control = channel->norm_input();
+
+    // normalize deadzone
+    auto dz = (float)g.throttle_deadzone * 2.0f / (float)(channel->get_radio_max() - channel->get_radio_min());
+    auto deadband_top = dz * gain;
+    auto deadband_bottom = -dz * gain;
+
+    if (control < deadband_bottom) {
+        // below the deadband
+        return (float)g.pilot_speed * (control - deadband_bottom);
+    } else if (control > deadband_top) {
+        // above the deadband
+        return (float)g.pilot_speed * (control - deadband_top);
+    } else {
+        // must be in the deadband
+        return 0;
+    }
+}
+
 // rotate vector from vehicle's perspective to North-East frame
 void Sub::rotate_body_frame_to_NE(float &x, float &y)
 {

ArduSub/Parameters.cpp

@@ -175,6 +175,15 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(pilot_speed_dn,     "PILOT_SPEED_DN",   0),
 
+    // @Param: PILOT_SPEED
+    // @DisplayName: Pilot maximum horizontal speed
+    // @Description: The maximum horizontal velocity the pilot may request in cm/s
+    // @Units: cm/s
+    // @Range: 10 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(pilot_speed,     "PILOT_SPEED",   PILOT_SPEED_DEFAULT),
+
     // @Param: PILOT_ACCEL_Z
     // @DisplayName: Pilot vertical acceleration
     // @Description: The vertical acceleration used when pilot is controlling the altitude

ArduSub/Parameters.h

@@ -231,6 +231,7 @@ public:
         k_param_pilot_speed_dn,
         k_param_rangefinder_signal_min,
         k_param_surftrak_depth,
+        k_param_pilot_speed,
 
         k_param_vehicle = 257, // vehicle common block of parameters
     };
@@ -269,6 +270,7 @@ public:
     //
     AP_Int16        pilot_speed_up;             // maximum vertical ascending velocity the pilot may request
     AP_Int16        pilot_speed_dn;             // maximum vertical descending velocity the pilot may request
+    AP_Int16        pilot_speed;                // maximum horizontal (xy) velocity the pilot may request
     AP_Int16        pilot_accel_z;              // vertical acceleration the pilot may request
 
     // Throttle

ArduSub/Sub.h

@@ -403,6 +403,7 @@ private:
     float get_roi_yaw();
     float get_look_ahead_yaw();
     float get_pilot_desired_climb_rate(float throttle_control);
+    float get_pilot_desired_horizontal_rate(RC_Channel *channel) const;
     void rotate_body_frame_to_NE(float &x, float &y);
 #if HAL_LOGGING_ENABLED
     // methods for AP_Vehicle:

ArduSub/config.h

@@ -167,10 +167,13 @@
 # define THR_DZ_DEFAULT         100             // the deadzone above and below mid throttle while in althold or loiter
 #endif
 
-// default maximum vertical velocity and acceleration the pilot may request
+// default maximum velocities and acceleration the pilot may request
 #ifndef PILOT_VELZ_MAX
 # define PILOT_VELZ_MAX    500     // maximum vertical velocity in cm/s
 #endif
+#ifndef PILOT_SPEED_DEFAULT
+# define PILOT_SPEED_DEFAULT 200 // maximum horizontal velocity in cm/s while under pilot control
+#endif
 #ifndef PILOT_ACCEL_Z_DEFAULT
 # define PILOT_ACCEL_Z_DEFAULT 100 // vertical acceleration in cm/s/s while altitude is under pilot control
 #endif

ArduSub/mode.h

@@ -429,6 +429,10 @@ protected:
     const char *name() const override { return "POSHOLD"; }
     const char *name4() const override { return "POSH"; }
     Mode::Number number() const override { return Mode::Number::POSHOLD; }
+
+private:
+
+    void control_horizontal();
 };
 
 

ArduSub/mode_poshold.cpp

@@ -15,8 +15,8 @@ bool ModePoshold::init(bool ignore_checks)
     }
 
     // initialize vertical speeds and acceleration
-    position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
+    position_control->set_max_speed_accel_xy(g.pilot_speed, g.pilot_accel_z);
-    position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
+    position_control->set_correction_speed_accel_xy(g.pilot_speed, g.pilot_accel_z);
     position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
     position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 
@@ -54,26 +54,6 @@ void ModePoshold::run()
     // set motors to full range
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    ///////////////////////
-    // update xy outputs //
-    float pilot_lateral = channel_lateral->norm_input();
-    float pilot_forward = channel_forward->norm_input();
-
-    float lateral_out = 0;
-    float forward_out = 0;
-
-    if (sub.position_ok()) {
-        // Allow pilot to reposition the sub
-        if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
-            position_control->init_xy_controller_stopping_point();
-        }
-        sub.translate_pos_control_rp(lateral_out, forward_out);
-        position_control->update_xy_controller();
-    } else {
-        position_control->init_xy_controller_stopping_point();
-    }
-    motors.set_forward(forward_out + pilot_forward);
-    motors.set_lateral(lateral_out + pilot_lateral);
     /////////////////////
     // Update attitude //
 
@@ -103,12 +83,51 @@ void ModePoshold::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);
         }
     }
 
-    // Update z axis //
+    // update z axis
     control_depth();
+
+    // update xy axis
+    // call this after Sub::get_pilot_desired_climb_rate is called so that THR_DZ is reasonable
+    control_horizontal();
+}
+
+void ModePoshold::control_horizontal() {
+    float lateral_out = 0;
+    float forward_out = 0;
+
+    // get desired rates in the body frame
+    Vector2f body_rates_cm_s = {
+        sub.get_pilot_desired_horizontal_rate(channel_forward),
+        sub.get_pilot_desired_horizontal_rate(channel_lateral)
+    };
+
+    if (sub.position_ok()) {
+        if (!position_control->is_active_xy()) {
+            // the xy controller timed out, re-initialize
+            position_control->init_xy_controller_stopping_point();
+        }
+
+        // convert to the earth frame and set target rates
+        auto earth_rates_cm_s = ahrs.body_to_earth2D(body_rates_cm_s);
+        position_control->input_vel_accel_xy(earth_rates_cm_s, {0, 0});
+
+        // convert pos control roll and pitch angles back to lateral and forward efforts
+        sub.translate_pos_control_rp(lateral_out, forward_out);
+
+        // udpate the xy controller
+        position_control->update_xy_controller();
+    } else if (g.pilot_speed > 0) {
+        // allow the pilot to reposition manually
+        forward_out = body_rates_cm_s.x / (float)g.pilot_speed;
+        lateral_out = body_rates_cm_s.y / (float)g.pilot_speed;
+    }
+
+    motors.set_forward(forward_out);
+    motors.set_lateral(lateral_out);
 }
 #endif  // POSHOLD_ENABLED