diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 10:59:35 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 10:59:35 +0200 |
| commit | dd1945bb7644b41e765866aefd3c6cc14e433b37 (patch) | |
| tree | ecd1f922c6d40a5f58187aabb8eeac11d8f5bbd4 /src/lib/external_lgpl/tecs/tecs.cpp | |
| parent | ee2eb98167c282aea88714e1a9beea9e523f2e55 (diff) | |
| download | px4-firmware-dd1945bb7644b41e765866aefd3c6cc14e433b37.tar.gz px4-firmware-dd1945bb7644b41e765866aefd3c6cc14e433b37.tar.bz2 px4-firmware-dd1945bb7644b41e765866aefd3c6cc14e433b37.zip | |
add heightrate ff for tecs
Diffstat (limited to 'src/lib/external_lgpl/tecs/tecs.cpp')
| -rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 2 |
1 files changed, 1 insertions, 1 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776f..023bd71bf 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; |