From dc612d75c788bc8b2a90c200a13a5a5a0e3a68d9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 18 Jul 2014 00:15:16 +0200
Subject: [PATCH] BlockDerivative: initialize in first run

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

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index c6c374300e..04cb023a8d 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -293,7 +293,12 @@ int blockIntegralTrapTest()
 
 float BlockDerivative::update(float input)
 {
-	float output = _lowPass.update((input - getU()) / getDt());
+	float output = 0.0f;
+	if (_initialized) {
+		output = _lowPass.update((input - getU()) / getDt());
+	} else {
+		_initialized = true;
+	}
 	setU(input);
 	return output;
 }
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 66e9290381..b0545b60a5 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -238,6 +238,7 @@ public:
 	BlockDerivative(SuperBlock *parent, const char *name) :
 		SuperBlock(parent, name),
 		_u(0),
+		_initialized(false),
 		_lowPass(this, "LP")
 	{};
 	virtual ~BlockDerivative() {};
@@ -249,6 +250,7 @@ public:
 protected:
 // attributes
 	float _u; /**< previous input */
+	bool _initialized;
 	BlockLowPass _lowPass; /**< low pass filter */
 };