diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index e595464db3..66db6bbf0e 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -113,7 +113,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) // Multiply roll rate error by _ki_rate and integrate // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs - if (ki_rate > 0) { + if (ki_rate > 0 && speed >= _minspeed) { // only integrate if gain and time step are positive. if (dt > 0) { float integrator_delta = rate_error * ki_rate * delta_time * scaler;