From 6898ec57764b791575c7b3f00c51ffae6a6ccc0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Mar 2019 21:49:13 +1100 Subject: [PATCH] AP_TECS: prevent airspeed demand spikes causing large pitch changes a short term spike in the derivative of speed demand could cause the constraint on the pitch integrator to push the pitch integrator to very low values, causing a sharp nose down which takes a long time to recover from --- libraries/AP_TECS/AP_TECS.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 6126f8d289..d49a48699e 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -858,6 +858,15 @@ void AP_TECS::_update_pitch(void) // causing massive integrator changes. See Issue#4066 integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f); + // prevent the constraint on pitch integrator _integSEB_state from + // itself injecting step changes in the variable. We only want the + // constraint to prevent large changes due to integSEB_delta, not + // to cause step changes due to a change in the constrain + // limits. Large steps in _integSEB_state can cause long term + // pitch changes + integSEB_min = MIN(integSEB_min, _integSEB_state); + integSEB_max = MAX(integSEB_max, _integSEB_state); + // integrate _integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max);