From b4171853b1d33191fc03c5ea51b8562798c23111 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 18 Jan 2014 11:57:59 +1100
Subject: [PATCH] AP_NavEKF: allow initialisation before GPS lock to aid indoor
 testing

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp |  4 ++--
 libraries/AP_NavEKF/AP_NavEKF.cpp    | 16 ++++++++++++----
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 4740d063d4..372ca4dfdc 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -52,8 +52,8 @@ void AP_AHRS_NavEKF::update(void)
     _dcm_attitude(roll, pitch, yaw);
 
     if (!ekf_started) {
-        // if we have GPS lock and a compass set we can start the EKF
-        if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
+        // if we have a compass set we can start the EKF
+        if (get_compass()) {
             if (start_time_ms == 0) {
                 start_time_ms = hal.scheduler->millis();
             }
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 8f7f72efbf..7ab30cfdc6 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -383,9 +383,9 @@ void NavEKF::UpdateFilter()
 
 void NavEKF::SelectVelPosFusion()
 {
-    // Command fusion of GPS measurements if new ones available
+    // Command fusion of GPS measurements if new ones available or in static mode
     readGpsData();
-    if (newDataGps) {
+    if (newDataGps||staticMode) {
         fuseVelData = true;
         fusePosData = true;
         // Calculate the scale factor to be applied to the measurement variance to account for
@@ -398,9 +398,9 @@ void NavEKF::SelectVelPosFusion()
         fuseVelData = false;
         fusePosData = false;
     }
-    // Command fusion of height measurements if new ones available
+    // Command fusion of height measurements if new ones available or in static mode
     readHgtData();
-    if (newDataHgt)
+    if (newDataHgt||staticMode)
     {
         fuseHgtData = true;
         // Calculate the scale factor to be applied to the measurement variance to account for
@@ -1337,6 +1337,10 @@ void NavEKF::FuseVelPosNED()
             {
                 posHealth = true;
                 posFailTime = hal.scheduler->millis();
+                if (posTimeout)
+                {
+                    ResetPosition();
+                }
             }
             else
             {
@@ -1360,6 +1364,10 @@ void NavEKF::FuseVelPosNED()
             {
                 hgtHealth = true;
                 hgtFailTime = hal.scheduler->millis();
+                if (hgtTimeout)
+                {
+                    ResetPosition();
+                }
             }
             else
             {