From d6508acfa5870843d617544765899ee81ca0444a Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 4 Nov 2014 08:59:51 +1100 Subject: [PATCH] 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 | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 911219a6d3..d8c7a8d3e1 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/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