aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_TECS : Prevent throttle undershoot after climb

Browse Source 
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.
This commit is contained in:
priseborough 2014-11-04 08:59:51 +11:00 committed by Andrew Tridgell
parent 3518cf5480
commit d6508acfa5
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_TECS/AP_TECS.cpp
View File

@ -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