From 97e9f44d357f35c31a313812acec699f38e74a18 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 22 Jul 2015 22:22:41 +0200
Subject: [PATCH] TECS: Reset integrator states on first run or when DT is
 large

---
 src/lib/external_lgpl/tecs/tecs.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index d13673ec9b..33beac14bb 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -45,7 +45,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
 	// 	accel_earth(0), accel_earth(1), accel_earth(2));
 
-	if (DT > 1.0f) {
+	if (_update_50hz_last_usec == 0 || DT > 1.0f) {
 		_integ3_state = baro_altitude;
 		_integ2_state = 0.0f;
 		_integ1_state = 0.0f;