From c610050dc305884a704b51c00cfff5f2f9984bcb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Jul 2014 14:55:23 +0200
Subject: EKF hotfix: Add missing vector zero calls

---
 src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

(limited to 'src/modules')

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index f33a1d780..3da92b264 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2280,7 +2280,7 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
     }
 
     if (!isfinite(val)) {
-        ekf_debug("constrain: non-finite!");
+        //ekf_debug("constrain: non-finite!");
     }
 
     return ret;
@@ -2920,6 +2920,8 @@ void AttPosEKF::ZeroVariables()
     correctedDelAng.zero();
     summedDelAng.zero();
     summedDelVel.zero();
+    dAngIMU.zero();
+    dVelIMU.zero();
     lastGyroOffset.zero();
 
     for (unsigned i = 0; i < data_buffer_size; i++) {
-- 
cgit v1.2.3