From 9a6ac77c64ac7e8c5ea74edf90dc40e174a626bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 May 2016 15:46:27 +1000 Subject: [PATCH] AP_TECS: prevent large glitches in the specific energy integrator this tries to prevent problems like in issue#4066, or at least reducing the impact when it happens --- libraries/AP_TECS/AP_TECS.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 130c56c817..f9ecb72452 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -788,7 +788,7 @@ void AP_TECS::_update_pitch(void) { integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem); } - _integSEB_state = _integSEB_state + integSEB_input * _DT; + float integSEB_delta = integSEB_input * _DT; #if 0 if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { @@ -820,7 +820,17 @@ void AP_TECS::_update_pitch(void) if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { temp += _PITCHminf * gainInv; } - _integSEB_state = constrain_float(_integSEB_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; + float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp; + float integSEB_range = integSEB_max - integSEB_min; + + // don't allow the integrator to rise by more than 20% of its full + // range in one step. This prevents single value glitches from + // causing massive integrator changes. See Issue#4066 + integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f); + + // integrate + _integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integSEB_state) / gainInv;