Copter: Separate max ascent and descent speeds

Added equivalent parameters to WPNAV_SPEED_UP and WPNAV_SPEED_DN 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). Flight Modes impacted: ALTHOLD AUTOTUNE CIRCLE LOITER POSHOLD SPORT TAKEOFF 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.
ArduPilot · Nov 8, 2017 · febf52b · febf52b
1 parent 7fb4f17
commit febf52b
Show file tree

Hide file tree

Showing 12 changed files with 50 additions and 26 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -200,10 +200,10 @@ float Copter::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;
@@ -307,3 +307,13 @@ void Copter::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 Copter::get_pilot_speed_dn()
+{
+    if (g2.pilot_speed_dn == 0) {
+        return abs(g.pilot_speed_up);
+    } else {
+        return abs(g2.pilot_speed_dn);
+    }
+}
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -1162,6 +1162,8 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void dataflash_periodic(void);
     void ins_periodic();
     void accel_cal_update(void);
+
+    uint16_t get_pilot_speed_dn();    
 
 public:
     void mavlink_delay_cb();

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
@@ -251,14 +251,14 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Standard
     GSCALAR(land_speed_high,        "LAND_SPEED_HIGH",   0),
 
-    // @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 speed ascending
+    // @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_ACCEL_Z
     // @DisplayName: Pilot vertical acceleration
@@ -1018,6 +1018,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Winch/AP_Winch.cpp
     AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch),
 
+    // @Param: PILOT_SPEED_DN
+    // @DisplayName: Pilot maximum vertical speed descending
+    // @Description: The maximum vertical descending velocity the pilot may request in cm/s
+    // @Units: cm/s
+    // @Range: 50 500
+    // @Increment: 10
+    // @User: Standard
+    AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0),
+
     AP_GROUPEND
 };
 

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
@@ -99,7 +99,7 @@ class Parameters {
         k_param_throttle_accel_enabled,     // deprecated - remove
         k_param_wp_yaw_behavior,
         k_param_acro_trainer,
-        k_param_pilot_velocity_z_max,
+        k_param_pilot_speed_up,    // renamed from k_param_pilot_velocity_z_max
         k_param_circle_rate,                // deprecated - remove
         k_param_rangefinder_gain,
         k_param_ch8_option,
@@ -414,7 +414,7 @@ class Parameters {
     AP_Int32        rtl_loiter_time;
     AP_Int16        land_speed;
     AP_Int16        land_speed_high;
-    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_accel_z;               // vertical acceleration the pilot may request
 
     // Throttle
@@ -574,6 +574,9 @@ class ParametersG2 {
     // wheel encoder and winch
     AP_WheelEncoder wheel_encoder;
     AP_Winch winch;
+
+    // Additional pilot velocity items
+    AP_Int16    pilot_speed_dn;
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp
@@ -9,7 +9,7 @@
 bool Copter::althold_init(bool ignore_checks)
 {
     // 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 position and desired velocity
@@ -32,7 +32,7 @@ void Copter::althold_run()
     float takeoff_climb_rate = 0.0f;
 
     // 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);
 
     // apply SIMPLE mode transform to pilot inputs
@@ -47,7 +47,7 @@ void Copter::althold_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);
 
 #if FRAME_CONFIG == HELI_FRAME
     // helicopters are held on the ground until rotor speed runup has finished

diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp
@@ -263,7 +263,7 @@ bool Copter::autotune_start(bool ignore_checks)
     }
 
     // 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 position and desired velocity
@@ -404,7 +404,7 @@ void Copter::autotune_run()
     autotune_do_gcs_announcements();
 
     // 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately

diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp
@@ -14,7 +14,7 @@ bool Copter::circle_init(bool ignore_checks)
         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 Copter::circle_run()
     // initialize speeds and accelerations
     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 auto armed or motor interlock not enabled set throttle to zero and exit immediately

diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp
@@ -110,7 +110,7 @@ void Copter::guided_vel_control_start()
     pos_control->set_jerk_xy_to_default();
 
     // 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 velocity controller

diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp
@@ -13,7 +13,7 @@ bool Copter::loiter_init(bool ignore_checks)
         wp_nav->init_loiter_target();
 
         // initialize vertical speed 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
@@ -74,7 +74,7 @@ void Copter::loiter_run()
     float takeoff_climb_rate = 0.0f;
 
     // initialize vertical speed 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);
 
     // process pilot inputs unless we are in radio failsafe
@@ -90,7 +90,7 @@ void Copter::loiter_run()
 
         // get pilot desired climb rate
         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);
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
         wp_nav->clear_pilot_desired_acceleration();

diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp
@@ -81,7 +81,7 @@ bool Copter::poshold_init(bool ignore_checks)
     }
 
     // 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
@@ -137,7 +137,7 @@ void Copter::poshold_run()
     const Vector3f& vel = inertial_nav.get_velocity();
 
     // 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
@@ -159,7 +159,7 @@ void Copter::poshold_run()
 
         // get pilot desired climb rate (for alt-hold mode and take-off)
         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);
 
         // get takeoff adjusted pilot and takeoff climb rates
         takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);

diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp
@@ -8,7 +8,7 @@
 bool Copter::sport_init(bool ignore_checks)
 {
     // initialize vertical speed 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
@@ -28,7 +28,7 @@ void Copter::sport_run()
     float takeoff_climb_rate = 0.0f;
 
     // initialize vertical speed 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);
 
     // apply SIMPLE mode transform
@@ -68,7 +68,7 @@ void Copter::sport_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);
 
 #if FRAME_CONFIG == HELI_FRAME
     // helicopters are held on the ground until rotor speed runup has finished

diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp
@@ -59,7 +59,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
 void Copter::takeoff_timer_start(float alt_cm)
 {
     // calculate climb rate
-    float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
+    float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_speed_up*2.0f/3.0f, g.pilot_speed_up-50.0f));
 
     // sanity check speed and target
     if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {