Sub: Separate max ascent and descent speeds

This is to match a change made to Copter.

It might be relevant to sub in that users may wish to have asymmetrical descent and ascent rates.

New parameters named:
PILOT_SPEED_UP (technically renamed PILOT_VELZ_MAX)
PILOT_SPEED_DN

Removed parameter PILOT_VELZ_MAX (technically renamed to PILOT_SPEED_UP).

Modes impacted:
ALT_HOLD
CIRCLE
POSHOLD

Update a section in GUIDED mode but I don't think it is ever used but update just in case.

It will use the PILOT_SPEED_UP for ascending max velocity.  For down it will check if
it is 0, if so then it will PILOT_SPEED_UP instead, if non zero it will use PILOT_SPEED_DN.
This retains current behavior and gives the flexibility to change it if desired.

The above behavior is less of a concern for Sub but to keep it consistent its been implemented it the same way.

ArduSub/Attitude.cpp

@@ -115,10 +115,10 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
     // check throttle is above, below or in the deadband
     if (throttle_control < deadband_bottom) {
         // below the deadband
-        desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom;
+        desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
     } else if (throttle_control > deadband_top) {
         // above the deadband
-        desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
+        desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
     } else {
         // must be in the deadband
         desired_rate = 0.0f;
@@ -200,3 +200,13 @@ void Sub::rotate_body_frame_to_NE(float &x, float &y)
     x = ne_x;
     y = ne_y;
 }
+
+// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
+uint16_t Sub::get_pilot_speed_dn()
+{
+    if (g.pilot_speed_dn == 0) {
+        return abs(g.pilot_speed_up);
+    } else {
+        return abs(g.pilot_speed_dn);
+    }
+}

ArduSub/Parameters.cpp

@@ -201,14 +201,23 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(wp_yaw_behavior,  "WP_YAW_BEHAVIOR",    WP_YAW_BEHAVIOR_DEFAULT),
 
-    // @Param: PILOT_VELZ_MAX
-    // @DisplayName: Pilot maximum vertical speed
-    // @Description: The maximum vertical velocity the pilot may request in cm/s
+    // @Param: PILOT_SPEED_UP
+    // @DisplayName: Pilot maximum vertical ascending speed
+    // @Description: The maximum vertical ascending velocity the pilot may request in cm/s
     // @Units: cm/s
     // @Range: 50 500
     // @Increment: 10
     // @User: Standard
-    GSCALAR(pilot_velocity_z_max,     "PILOT_VELZ_MAX",   PILOT_VELZ_MAX),
+    GSCALAR(pilot_speed_up,     "PILOT_SPEED_UP",   PILOT_VELZ_MAX),
+
+    // @Param: PILOT_SPEED_DN
+    // @DisplayName: Pilot maximum vertical descending speed
+    // @Description: The maximum vertical descending velocity the pilot may request in cm/s
+    // @Units: cm/s
+    // @Range: 50 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(pilot_speed_dn,     "PILOT_SPEED_DN",   0),
 
     // @Param: PILOT_ACCEL_Z
     // @DisplayName: Pilot vertical acceleration

ArduSub/Parameters.h

@@ -180,7 +180,7 @@ public:
         k_param_rangefinder_gain,
         k_param_wp_yaw_behavior = 170,
         k_param_xtrack_angle_limit, // Angle limit for crosstrack correction in Auto modes (degrees)
-        k_param_pilot_velocity_z_max,
+        k_param_pilot_speed_up,     // renamed from k_param_pilot_velocity_z_max
         k_param_pilot_accel_z,
         k_param_compass_enabled,
         k_param_surface_depth,
@@ -210,6 +210,7 @@ public:
 
         k_param_cam_slew_limit = 237, // deprecated
         k_param_lights_steps,
+        k_param_pilot_speed_dn,
 
     };
 
@@ -248,7 +249,8 @@ public:
 
     // Waypoints
     //
-    AP_Int16        pilot_velocity_z_max;        // maximum vertical velocity the pilot may request
+    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_accel_z;               // vertical acceleration the pilot may request
 
     // Throttle

ArduSub/Sub.h

@@ -729,6 +729,8 @@ private:
     bool surface_init(void);
     void surface_run();
 
+    uint16_t get_pilot_speed_dn();
+
     void convert_old_parameters(void);
 
 public:

ArduSub/control_althold.cpp

@@ -17,7 +17,7 @@ bool Sub::althold_init()
 
     // initialize vertical speeds and leash lengths
     // sets the maximum speed up and down returned by position controller
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
@@ -36,7 +36,7 @@ void Sub::althold_run()
     uint32_t tnow = AP_HAL::millis();
 
     // initialize vertical speeds and acceleration
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     if (!motors.armed()) {

ArduSub/control_circle.cpp

@@ -17,7 +17,7 @@ bool Sub::circle_init()
     pos_control.set_speed_xy(wp_nav.get_speed_xy());
     pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
     pos_control.set_jerk_xy_to_default();
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise circle controller including setting the circle center based on vehicle speed
@@ -36,7 +36,7 @@ void Sub::circle_run()
     // update parameters, to allow changing at runtime
     pos_control.set_speed_xy(wp_nav.get_speed_xy());
     pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // if not armed set throttle to zero and exit immediately

ArduSub/control_guided.cpp

@@ -77,7 +77,7 @@ void Sub::guided_vel_control_start()
     guided_mode = Guided_Velocity;
 
     // initialize vertical speeds and leash lengths
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise velocity controller

ArduSub/control_poshold.cpp

@@ -15,7 +15,7 @@ bool Sub::poshold_init()
     }
 
     // initialize vertical speeds and acceleration
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
@@ -110,7 +110,7 @@ void Sub::poshold_run()
 
     // get pilot desired climb rate
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
-    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+    target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
 
     // adjust climb rate using rangefinder
     if (rangefinder_alt_ok()) {