tecs: improvements to pitch integration limiting

Constrain the specific energy balance integrator input to prevent increasing saturation of pitch demand.
Decay the specific energy balance integrator state if the pitch demand is saturated to reduce saturation to zero and do so at the same tome constant as the control loop
Relax the clipping threshold on the specific energy balance integrator to allow the input constraint and decay functions to do more of the work
Improve variable naming and commenting

src/lib/external_lgpl/tecs/tecs.cpp

@@ -453,31 +453,33 @@ void TECS::_update_pitch(void)
 	// Calculate integrator state, constraining input if pitch limits are exceeded
 	float integ7_input = _SEB_error * _integGain;
 
+	// constrain the integrator input to prevent it changing in the direction that increases pitch demand saturation
+	// if the pitch demand is saturated, then decay the integrator at the control loop time constant
 	if (_pitch_dem_unc > _PITCHmaxf) {
-		integ7_input = min(integ7_input, 0.0f);
-		// integ7_input = min(integ7_input, (_PITCHmaxf - _pitch_dem_unc) * gainInv / _DT);
+		integ7_input = min(integ7_input, min((_PITCHmaxf - _pitch_dem_unc) * gainInv / _timeConst, 0.0f));
 
 	} else if (_pitch_dem_unc < _PITCHminf) {
-		integ7_input = max(integ7_input, 0.0f);
-		// integ7_input = max(integ7_input, (_PITCHminf - _pitch_dem_unc) * gainInv / _DT);
+		integ7_input = max(integ7_input, max((_PITCHminf - _pitch_dem_unc) * gainInv / _timeConst, 0.0f));
 	}
 
+	// pitch loop integration
 	_integ7_state = _integ7_state + integ7_input * _DT;
 
-	float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	// Specific Energy Balance correction excluding integrator contribution
+	float SEB_correction = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
 
 	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
 	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
 	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	if (_climbOutDem) {
-		temp += _PITCHminf * gainInv;
+		SEB_correction += _PITCHminf * gainInv;
 	}
 
 	// Apply max and min values for integrator state that will allow for no more than
-	// 5deg of saturation. This allows for some pitch variation due to gusts before the
+	// 10deg of saturation. This allows for some pitch variation due to gusts before the
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
-	float integ7_err_min = (gainInv * (_PITCHminf - math::radians(5.0f))) - temp;
-	float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp;
+	float integ7_err_min = (gainInv * (_PITCHminf - math::radians(10.0f))) - SEB_correction;
+	float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(10.0f))) - SEB_correction;
 	_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
@@ -488,7 +490,7 @@ void TECS::_update_pitch(void)
 	}
 
 	// Calculate pitch demand from specific energy balance signals
-	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
+	_pitch_dem_unc = (SEB_correction + _integ7_state) / gainInv;
 
 	// Constrain pitch demand
 	_pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);