mtecs: do not calculate derivative in first iteration

3 files changed, 11 insertions, 0 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 7fd7ed07f..acd623c30 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/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);
 			}
 		}
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 039fc34a8..a5e680f02 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/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) {
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 4643145c1..ddd6583e8 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/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;}