diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 64e972fd78..58b10068b5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2244,6 +2244,10 @@ void QuadPlane::run_xy_controller(float accel_limit) pos_control->init_xy_controller(); } pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_to_angle(accel_limit)*100, aparm.angle_max))); + if (q_fwd_throttle > 0.95f) { + // prevent wind up of the velocity controller I term due to a saturated forward throttle + pos_control->set_externally_limited_xy(); + } pos_control->update_xy_controller(); }