From 1d634cc2bed48eb790c8a6dc95ee7a9e21ea54c0 Mon Sep 17 00:00:00 2001 From: Ben Wolsieffer Date: Sat, 20 Mar 2021 16:37:07 -0400 Subject: [PATCH] AP_Control: apply pitch rate limit to turn coordination At high bank angles, for example when rolling to/from inverted, a large turn coordination pitch rate offset is requested. Before this patch, this offset was not subject to the configured pitch rate limit, which could result in pitch controller integrator windup. --- libraries/APM_Control/AP_PitchController.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) 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