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 1/2] 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 */
 };
 

From 6c50e510a5fcadab19a6d5ff709c0da473e4ace0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 18 Jul 2014 07:28:49 +0200
Subject: [PATCH 2/2] Derivative fix: Comments and code style

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

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 04cb023a8d..0175acda99 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 b0545b60a5..37d7832b33 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;}