mc_pos_control: reset velocity derivatives

src/lib/controllib/BlockDerivative.hpp

@@ -87,6 +87,7 @@ public:
 	float update(float input);
 // accessors
 	void setU(float u) {_u = u;}
+	void reset() { _initialized = false; };
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
 	float getO() { return _lowPass.getState(); }

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -702,6 +702,12 @@ MulticopterPositionControl::Run()
 			}
 
 			_old_landing_gear_position = gear.landing_gear;
+
+		} else {
+			// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
+			_vel_x_deriv.reset();
+			_vel_y_deriv.reset();
+			_vel_z_deriv.reset();
 		}
 	}