From 2bc74934b8417433a108a7808138c88c29d9d713 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sun, 12 Oct 2014 22:17:38 +1100
Subject: [PATCH] AP_NavEKF: Track baro alt when pre-armed

This will help prevent spurious alt disparity warning messages for copter
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 33 +++++++++++++++----------------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index dec4f387ff..de4fb68a3a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -702,23 +702,6 @@ void NavEKF::SelectVelPosFusion()
             fusePosData = false;
         }
 
-        // check for and read new height data
-        readHgtData();
-
-        // command fusion of height data
-        if (newDataHgt)
-        {
-            // reset data arrived flag
-            newDataHgt = false;
-            // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
-            memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
-            hgtUpdateCount = 0;
-            // enable fusion
-            fuseHgtData = true;
-        } else {
-            fuseHgtData = false;
-        }
-
     } else {
         // in static mode use synthetic position measurements set to zero
         // only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration
@@ -729,7 +712,23 @@ void NavEKF::SelectVelPosFusion()
             fusePosData = false;
         }
         fuseVelData = false;
+    }
+
+    // check for and read new height data
+    readHgtData();
+
+    // command fusion of height data
+    if (newDataHgt)
+    {
+        // reset data arrived flag
+        newDataHgt = false;
+        // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
+        memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
+        hgtUpdateCount = 0;
+        // enable fusion
         fuseHgtData = true;
+    } else {
+        fuseHgtData = false;
     }
 
     // perform fusion