From 4a7cc627b58866e6aa8f45d372962f41a64feff3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 22 Dec 2021 20:54:26 +1030 Subject: [PATCH] Copter: Constrain vertical speed in loiter_to_alt_run --- ArduCopter/mode_auto.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9940ea97e1..af35251065 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -979,6 +979,7 @@ void ModeAuto::loiter_to_alt_run() pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); + target_climb_rate = constrain_float(target_climb_rate, pos_control->get_max_speed_down_cms(), pos_control->get_max_speed_up_cms()); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);