diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-08 12:45:09 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-08 12:45:09 +0200 |
| commit | 33c0cd2e569782ed68931b6ffd6be4a45c6b1b54 (patch) | |
| tree | 84acdd63294d32a3430943bba99364b8c7d2e7b7 /src/lib | |
| parent | daf16184202b02119aa1ac83cb82cf85979d43f9 (diff) | |
| parent | cf601c09bf14ac4141bc66f2f71934fcf65da2a6 (diff) | |
| download | px4-firmware-33c0cd2e569782ed68931b6ffd6be4a45c6b1b54.tar.gz px4-firmware-33c0cd2e569782ed68931b6ffd6be4a45c6b1b54.tar.bz2 px4-firmware-33c0cd2e569782ed68931b6ffd6be4a45c6b1b54.zip | |
Merge remote-tracking branch 'private_swissfang/master' into launchpitchlimit_swissfang
Diffstat (limited to 'src/lib')
| -rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 3 | ||||
| -rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 6 |
2 files changed, 7 insertions, 2 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776f..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_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; - - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 36ae4ecaf..8ac31de6f 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -47,6 +47,7 @@ public: _rollComp(0.0f), _spdWeight(0.5f), _heightrate_p(0.0f), + _heightrate_ff(0.0f), _speedrate_p(0.0f), _throttle_dem(0.0f), _pitch_dem(0.0f), @@ -220,6 +221,10 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } @@ -256,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 |