From 040d1da8a146727cf9e581583845cb45e18a582f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 6 Sep 2015 15:46:59 +0200
Subject: [PATCH] add 2nd order low pass block

---
 src/modules/controllib/blocks.cpp |  8 ++++++++
 src/modules/controllib/blocks.hpp | 28 ++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index f739446fa7c..ed6bfb0f85c 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 979b9541be5..ded54e99886 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -45,6 +45,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <mathlib/math/test/test.hpp>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #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