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

TECS: hande pitch and throttle integrator saturation consistently 

- don't decay integrator during saturation but prevent it from being learned
into the direction which causes more actuator saturation

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-10-30 07:23:53 +03:00 committed by Roman Bapst
parent 77627a08ca
commit 97cc1e25dd
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -429,12 +429,11 @@ void TECS::_update_pitch_setpoint()
float pitch_integ_input = _SEB_rate_error * _integrator_gain_pitch;
// Prevent the integrator changing in a direction that will increase pitch demand saturation
// Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated
if (_pitch_setpoint_unc > _pitch_setpoint_max) {
pitch_integ_input = min(pitch_integ_input, (_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
pitch_integ_input = min(pitch_integ_input, 0.f);
} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
pitch_integ_input = max(pitch_integ_input, (_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
pitch_integ_input = max(pitch_integ_input, 0.f);
}
// Update the pitch integrator state.