diff options
author | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-04 11:10:14 +0100 |
---|---|---|
committer | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-06 11:00:14 +0100 |
commit | f414eef042be7ebf641638009b3676523bf4bed6 (patch) | |
tree | fed8e0e66aa720a663b6a5ba71bb488b69a7d3eb | |
parent | 2f715b74dd4866945e7ebe6aab735cd1e58fe01d (diff) | |
download | px4-firmware-f414eef042be7ebf641638009b3676523bf4bed6.tar.gz px4-firmware-f414eef042be7ebf641638009b3676523bf4bed6.tar.bz2 px4-firmware-f414eef042be7ebf641638009b3676523bf4bed6.zip |
TECS: Modify absolute-value limiting of throttle demand
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 14 |
1 files changed, 6 insertions, 8 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0ed210cf4..cfcc48b62 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; - _throttle_dem = constrain(_throttle_dem, - _last_throttle_dem - thrRateIncr, - _last_throttle_dem + thrRateIncr); + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); } // Ensure _last_throttle_dem is always initialized properly - // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! _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 + // Set to a value that 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, constraining state - // Set integrator to a max throttle value dduring climbout + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { |