BlockDerivative: initialize in first run

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;
 }

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