diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index cf96328287..48ef93d2f7 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -301,19 +301,16 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool // as the rates will be tuned when upright, and it is common that // much higher rates are needed inverted if (!inverted) { + desired_rate += rate_offset; if (gains.rmax_neg && desired_rate < -gains.rmax_neg) { desired_rate = -gains.rmax_neg; } else if (gains.rmax_pos && desired_rate > gains.rmax_pos) { desired_rate = gains.rmax_pos; } + } else { + // Make sure not to invert the turn coordination offset + desired_rate = -desired_rate + rate_offset; } - - if (inverted) { - desired_rate = -desired_rate; - } - - // Apply the turn correction offset - desired_rate = desired_rate + rate_offset; /* when we are past the users defined roll limit for the aircraft