From e3da7f564f1bc7746ba3be946fd02b95642cbf6e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 29 Jul 2014 21:36:55 +0200
Subject: [PATCH] NaN check and better init in lowpass

---
 src/modules/controllib/blocks.cpp | 3 +++
 src/modules/controllib/blocks.hpp | 2 +-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 0175acda99..f739446fa7 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 37d7832b33..bffc355a82 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() {};