mtecs: do not calculate derivative in first iteration

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -861,6 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
 

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -261,6 +261,12 @@ void mTecs::resetIntegrators()
 	_firstIterationAfterReset = true;
 }
 
+void mTecs::resetDerivatives(float airspeed)
+{
+	_airspeedDerivative.setU(airspeed);
+}
+
+
 void mTecs::updateTimeMeasurement()
 {
 	if (!_dtCalculated) {

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -139,6 +139,10 @@ public:
 	 */
 	void resetIntegrators();
 
+	/*
+	 * Reset all derivative calculations
+	 */
+	void resetDerivatives(float airspeed);
 
 	/* Accessors */
 	bool getEnabled() {return _mTecsEnabled.get() > 0;}