Copter: Constrain vertical speed in loiter_to_alt_run

This commit is contained in:
Leonard Hall 2021-12-22 20:54:26 +10:30 committed by Randy Mackay
parent 9ce42a067f
commit 0d035891fe
1 changed files with 1 additions and 0 deletions

View File

@ -979,6 +979,7 @@ void ModeAuto::loiter_to_alt_run()
pos_control->get_pos_z_p().kP(), pos_control->get_pos_z_p().kP(),
pos_control->get_max_accel_z_cmss(), pos_control->get_max_accel_z_cmss(),
G_Dt); 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 // get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);