diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 12:32:10 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 12:32:10 +0200 |
| commit | 4e9a52fe45655aa853bf9af10223d32767bb60c4 (patch) | |
| tree | 5826c74bce5f10401ee118ce8499b5a3904fbd03 /src/lib | |
| parent | dd1945bb7644b41e765866aefd3c6cc14e433b37 (diff) | |
| download | px4-firmware-4e9a52fe45655aa853bf9af10223d32767bb60c4.tar.gz px4-firmware-4e9a52fe45655aa853bf9af10223d32767bb60c4.tar.bz2 px4-firmware-4e9a52fe45655aa853bf9af10223d32767bb60c4.zip | |
heightrate ff: fix order of calculations
Diffstat (limited to 'src/lib')
| -rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 3 |
1 files changed, 1 insertions, 2 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 023bd71bf..da99aa5b1 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _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 + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; + _hgt_dem_adj_last = _hgt_dem_adj; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; |