diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-18 07:30:10 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-18 07:30:10 +0200 |
commit | 4b6192f0bc00f525d65587b3400dbe9a18d156d5 (patch) | |
tree | 2b660830ab787cdb994e7081d01140ac04aa4ab4 | |
parent | 1fea1a6015a0af829a440b2bba176488d6e6a0a3 (diff) | |
parent | 6c50e510a5fcadab19a6d5ff709c0da473e4ace0 (diff) | |
download | px4-firmware-4b6192f0bc00f525d65587b3400dbe9a18d156d5.tar.gz px4-firmware-4b6192f0bc00f525d65587b3400dbe9a18d156d5.tar.bz2 px4-firmware-4b6192f0bc00f525d65587b3400dbe9a18d156d5.zip |
Merge pull request #1203 from PX4/initderivativeblock
BlockDerivative: initialize in first run
-rw-r--r-- | src/modules/controllib/blocks.cpp | 13 | ||||
-rw-r--r-- | src/modules/controllib/blocks.hpp | 17 |
2 files changed, 29 insertions, 1 deletions
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index c6c374300..0175acda9 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,7 +293,18 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = _lowPass.update((input - getU()) / getDt()); + float output; + if (_initialized) { + output = _lowPass.update((input - getU()) / getDt()); + } else { + // if this is the first call to update + // we have no valid derivative + // and so we use the assumption the + // input value is not changing much, + // which is the best we can do here. + output = 0.0f; + _initialized = true; + } setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 66e929038..37d7832b3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -238,9 +238,25 @@ public: BlockDerivative(SuperBlock *parent, const char *name) : SuperBlock(parent, name), _u(0), + _initialized(false), _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ float update(float input); // accessors void setU(float u) {_u = u;} @@ -249,6 +265,7 @@ public: protected: // attributes float _u; /**< previous input */ + bool _initialized; BlockLowPass _lowPass; /**< low pass filter */ }; |