From 1651980b9faf2cbd83220b6e42ecfdbebcff6ea4 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 21 Jan 2014 06:41:41 +1100
Subject: [PATCH] AP_NavEKF: Improved behaviour following loss and regaining of
 GPS

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 33 +++++++++++++++++++++++++++----
 libraries/AP_NavEKF/AP_NavEKF.h   | 10 +++++++---
 2 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index f4db8babe6..1a16c255a0 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -263,7 +263,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
 
     // @Param: HGT_RETRY1
     // @DisplayName: Height retry time with vertical velocity measurement (msec)
-    // @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing us of the measurements. This value applies when GPS vertical velocity measurements are being used (EKF_GPS_TYPE = 0).
+    // @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing use of the measurements. This value applies when GPS vertical velocity measurements are being used (EKF_GPS_TYPE = 0).
     // @Range: 1000 - 30000
     // @Increment: 1000
     // @User: advanced
@@ -271,7 +271,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
 
     // @Param: HGT_RETRY2
     // @DisplayName: Height retry time without vertical velocity measurement (msec)
-    // @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing us of the measurements. This value applies when GPS vertical velocity measurements are NOT being used (EKF_GPS_TYPE = 1 or 2).
+    // @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing use of the measurements. This value applies when GPS vertical velocity measurements are NOT being used (EKF_GPS_TYPE = 1 or 2).
     // @Range: 1000 - 30000
     // @Increment: 1000
     // @User: advanced
@@ -356,10 +356,30 @@ void NavEKF::ResetPosition(void)
 {
     // read the GPS
     readGpsData();
-    readHgtData();
     // write to state vector
     states[7] = posNE[0];
     states[8] = posNE[1];
+}
+
+void NavEKF::ResetVelocity(void)
+{
+    // read the GPS
+    readGpsData();
+    // write to state vector
+    if (_fusionModeGPS <= 1) {
+        states[4] = velNED[0];
+        states[5] = velNED[1];
+    }
+    if (_fusionModeGPS <= 0) {
+        states[6] = velNED[2];
+    }
+}
+
+void NavEKF::ResetHeight(void)
+{
+    // read the altimeter
+    readHgtData();
+    // write to state vector
     states[9] = -hgtMea;
 }
 
@@ -1539,6 +1559,11 @@ void NavEKF::FuseVelPosNED()
             {
                 velHealth = true;
                 velFailTime = hal.scheduler->millis();
+                if (velTimeout)
+                {
+                    ResetVelocity();
+                }
+
             }
             else
             {
@@ -1588,7 +1613,7 @@ void NavEKF::FuseVelPosNED()
                 hgtFailTime = hal.scheduler->millis();
                 if (hgtTimeout)
                 {
-                    ResetPosition();
+                    ResetHeight();
                 }
             }
             else
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index dbd451f422..7a132a7f4e 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -80,9 +80,6 @@ public:
     // Initialise the attitude from accelerometer and magnetometer data (if present)
     void InitialiseFilterBootstrap(void);
 
-    // Reset the position and height states to the last GPS and barometer measurement
-    void ResetPosition(void);
-
     // inhibits position and velocity attitude corrections when set to true
     // setting to true has same effect as ahrs.set_correct_centrifugal(false)
     void SetStaticMode(bool setting);
@@ -217,6 +214,13 @@ private:
 
     void ZeroVariables();
 
+    void ResetPosition(void);
+
+    void ResetVelocity(void);
+
+    void ResetHeight(void);
+
+
 private:
     // EKF Mavlink Tuneable Parameters
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s