diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-29 21:36:55 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-29 21:36:55 +0200 |
| commit | e3da7f564f1bc7746ba3be946fd02b95642cbf6e (patch) | |
| tree | b69ee5fe005979cdc00b025388f598e631b08eee /src/modules/controllib | |
| parent | 5422e081246885331d4e34fbb0fbcdf1587b25d3 (diff) | |
| download | px4-firmware-e3da7f564f1bc7746ba3be946fd02b95642cbf6e.tar.gz px4-firmware-e3da7f564f1bc7746ba3be946fd02b95642cbf6e.tar.bz2 px4-firmware-e3da7f564f1bc7746ba3be946fd02b95642cbf6e.zip | |
NaN check and better init in lowpass
Diffstat (limited to 'src/modules/controllib')
| -rw-r--r-- | src/modules/controllib/blocks.cpp | 3 | ||||
| -rw-r--r-- | src/modules/controllib/blocks.hpp | 2 |
2 files changed, 4 insertions, 1 deletions
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 0175acda9..f739446fa 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 37d7832b3..bffc355a8 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; |