Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Move the throttle rate limit code to ensure rate limit is respected when exiting takeoff stage.

Browse Source
This commit is contained in:
Samuel Tabor 2020-03-03 18:12:59 +00:00 committed by Tom Pittenger
parent 0b8f722dbd
commit 88d4aa34db
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_TECS/AP_TECS.cpp
View File

@ -674,17 +674,6 @@ void AP_TECS::_update_throttle_with_airspeed(void)
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf); float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _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_clipped_to_zero) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Calculate integrator state upper and lower limits // 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 // 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. // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
@ -708,6 +697,17 @@ void AP_TECS::_update_throttle_with_airspeed(void)
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
} }
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Sum the components. // Sum the components.
_throttle_dem = _throttle_dem + _integTHR_state; _throttle_dem = _throttle_dem + _integTHR_state;
} }