diff options
Diffstat (limited to 'src/modules/controllib')
-rw-r--r-- | src/modules/controllib/blocks.cpp | 8 | ||||
-rw-r--r-- | src/modules/controllib/blocks.hpp | 15 |
2 files changed, 22 insertions, 1 deletions
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 04cb023a8..0175acda9 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,10 +293,16 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = 0.0f; + 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); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index b0545b60a..37d7832b3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -242,6 +242,21 @@ public: _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;} |