AP_TECS : Prevent throttle undershoot after climb

If the plane was unable to achieve the climb and got significantly below the internal TECS demanded climb profile, the the PD term would be a large value at the top of climb, and would take some time to reduce due to the rate limiter. This meant that the integrator state could be pushed to a very low value and effectively cause the throttle to sit on the lower limit for longer than desired after levelling out.

libraries/AP_TECS/AP_TECS.cpp

@@ -476,7 +476,10 @@ void AP_TECS::_update_throttle(void)
 		// Calculate PD + FF throttle
 		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
 
-		// Rate limit PD + FF throttle
+        // Constrain throttle demand
+        _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
+
+        // Rate limit PD + FF throttle
 	    // Calculate the throttle increment from the specified slew time
 		if (aparm.throttle_slewrate != 0) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f;
@@ -487,11 +490,12 @@ void AP_TECS::_update_throttle(void)
 			_last_throttle_dem = _throttle_dem;
 		}
 
-
-		// Calculate integrator state upper and lower limits
-		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
-        float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
-		float integ_min = (_THRminf - _throttle_dem - 0.1f);
+        // Calculate integrator state upper and lower limits
+        // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+        // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
+        float maxAmp = 0.5f*(_THRmaxf - _THRminf);
+        float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
+        float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
 
   		// Calculate integrator state, constraining state
 		// Set integrator to a max throttle value during climbout