dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

tecs: dot not clip throttle integrator during saturation but only allow 

it to propagate into a direction which does not saturate

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-10-25 08:22:54 +03:00 committed by Roman Bapst
parent 2ca4269464
commit 5c4c2c240c
1 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/lib/tecs/TECS.cpp
View File

@ -329,15 +329,24 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
if (_integrator_gain_throttle > 0.0f) {
// Calculate throttle integrator state upper and lower limits with allowance for
// 10% throttle saturation to accommodate noise on the demand.
float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f;
float integ_state_min = _throttle_setpoint_min - throttle_setpoint - 0.1f;
// Calculate a throttle demand from the integrated total energy error
float integ_state_max = _throttle_setpoint_max - throttle_setpoint;
float integ_state_min = _throttle_setpoint_min - throttle_setpoint;
float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt *
STE_rate_to_throttle;
// only allow integrator propagation into direction which unsaturates throttle
if (_throttle_integ_state > integ_state_max) {
throttle_integ_input = math::min(0.f, throttle_integ_input);
} else if (_throttle_integ_state < integ_state_min) {
throttle_integ_input = math::max(0.f, throttle_integ_input);
}
// Calculate a throttle demand from the integrated total energy rate error
// This will be added to the total throttle demand to compensate for steady state errors
_throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain_throttle) * _dt *
STE_rate_to_throttle;
_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
if (_climbout_mode_active) {
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop