From 12ddf9d25ec0ebda169c8bcdee25e4f96a328020 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 1 Nov 2016 14:36:20 +0100 Subject: [PATCH] TECS: better handling of the constraint for the pitch integrator - if the specific energy balance correction term produced a demanded pitch value which exceeded the aircraft pitch limits then the pitch integrator was shifted such that the pitch demand violation was prevented. However, this meant that the exceeding pitch was just unloaded into the integrator and caused unexpected behavior of the pitch loop. In an underspeed condition e.g. this has lead to the plane pulling up it's nose very quickly shorty after the underspeed condition kicked in. Signed-off-by: Roman --- src/lib/external_lgpl/tecs/tecs.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 10dd58150b..8c9ac8cf76 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -474,6 +474,13 @@ void TECS::_update_pitch(void) float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp; _integ7_state = constrain(_integ7_state, integ7_err_min, integ7_err_max); + // if the specific engergy balance correction term produces a demanded pitch value which exceeds the aircraft pitch limits + // then zero the integrator. Not doing so can lead to the integrator value being shifted to large unwanted values due to the + // constraint right above this comment + if (SEB_correction / gainInv > (_PITCHmaxf + math::radians(10.0f)) || SEB_correction / gainInv < (_PITCHminf - math::radians(10.0f))) { + _integ7_state = 0.0f; + } + // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; @@ -592,7 +599,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate the height demand _update_height_demand(hgt_dem, baro_altitude); - // Calculate specific energy quantitiues _update_energies();