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 <bapstroman@gmail.com>

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();