From 316541017abcf866424649ffbb22149f428b0b32 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Apr 2021 12:00:44 +1100 Subject: [PATCH] APM_Control: fixed handling of clipped actuator in autotune --- libraries/APM_Control/AP_AutoTune.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 52ce2a8deb..4821284d64 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -177,9 +177,11 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler) ATState new_state = state; const float desired_rate = pinfo.target; - // filter actuator without I term so we can take ratios without accounting - // for trim offsets - const float actuator = actuator_filter.apply(pinfo.FF + pinfo.P + pinfo.D); + // filter actuator without I term so we can take ratios without + // accounting for trim offsets. We first need to include the I and + // clip to 45 degrees to get the right value of the real surface + const float clipped_actuator = constrain_float(pinfo.FF + pinfo.P + pinfo.D + pinfo.I, -45, 45) - pinfo.I; + const float actuator = actuator_filter.apply(clipped_actuator); const float actual_rate = rate_filter.apply(pinfo.actual); max_actuator = MAX(max_actuator, actuator);