Derivative fix: Comments and code style

2 files changed, 22 insertions, 1 deletions

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 04cb023a8..0175acda9 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 b0545b60a..37d7832b3 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;}