From 7a82746fccef9dfdeb836e1ff1136a3077bba560 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 16 Feb 2014 22:32:17 +1100
Subject: [PATCH] AP_NavEKF : fixed bug in pos and vel reset when in static
 mode

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index cac7e22c2f..0d405d3eb7 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -557,8 +557,10 @@ void NavEKF::SelectVelPosFusion()
         skipCounter = velPosFuseStepRatio;
         // If a long time sinc last GPS update, then reset position and velocity
         if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) {
-            ResetPosition();
-            ResetVelocity();
+            if (!staticMode) {
+                ResetPosition();
+                ResetVelocity();
+            }
         }
     }
 
@@ -1515,7 +1517,9 @@ void NavEKF::FuseVelPosNED()
                 velFailTime = hal.scheduler->millis();
                 if (velTimeout)
                 {
-                    ResetVelocity();
+                    if (!staticMode) {
+                        ResetVelocity();
+                    }
                     fuseVelData =  false;
                 }
 
@@ -1541,7 +1545,9 @@ void NavEKF::FuseVelPosNED()
                 posFailTime = hal.scheduler->millis();
                 if (posTimeout)
                 {
-                    ResetPosition();
+                    if (!staticMode) {
+                        ResetPosition();
+                    }
                     fusePosData = false;
                 }
             }