diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index f739446fa7..ed6bfb0f85 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -198,6 +198,14 @@ int blockHighPassTest() return 0; } +float BlockLowPass2::update(float input) +{ + if (_lp.get_cutoff_freq() != getFCutParam()) { + _lp.set_cutoff_frequency(_fs, getFCutParam()); + } + return _lp.apply(input); +} + float BlockIntegral::update(float input) { // trapezoidal integration diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 979b9541be..ded54e9988 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include "block/Block.hpp" #include "block/BlockParam.hpp" @@ -163,6 +164,33 @@ protected: int __EXPORT blockHighPassTest(); +/** + * A 2nd order low pass filter block which uses the 2nd order low pass filter used by px4 + */ +class __EXPORT BlockLowPass2 : public Block +{ +public: +// methods + BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : + Block(parent, name), + _fCut(this, ""), // only one parameter, no need to name + _fs(sample_freq), + _lp(_fs, _fCut.get()) + {}; + virtual ~BlockLowPass2() {}; + float update(float input); +// accessors + float getFCutParam() { return _fCut.get(); } + void setState(float state) { _lp.reset(state); } +protected: +// attributes + control::BlockParamFloat _fCut; + float _fs; + math::LowPassFilter2p _lp; +}; + +// XXX missing test function for BlockLowPass2 + /** * A rectangular integrator. * A limiter is built into the class to bound the