From 7ae86b397924fd66d3a9dc9b11f888369bef07ac Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sun, 23 Mar 2014 15:04:20 +1100
Subject: [PATCH] AP_NavEKF : Track baro height observations pre-arm

This prevents copter from failing the pre-arm height discrepancy test
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index e6ce23ec67..f9a8a13054 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -641,9 +641,6 @@ void NavEKF::SelectVelPosFusion()
             fusePosData = false;
         }
 
-        // check for and read new height data
-        readHgtData();
-
         // command fusion of height data
         if (newDataHgt)
         {
@@ -660,7 +657,7 @@ void NavEKF::SelectVelPosFusion()
     } else {
         // in static mode we only fuse position and height to improve long term numerical stability
         // and only when the rate of change of velocity is less than 0.5g. This prevents attitude errors
-        // due to launch acceleration
+        // due to launch acceleration. position is assumed to be zero.
         fuseVelData = false;
         if (accNavMag < 4.9f) {
             fusePosData = true;
@@ -671,6 +668,9 @@ void NavEKF::SelectVelPosFusion()
         }
     }
 
+    // check for and read new height data
+    readHgtData();
+
     // perform fusion as commanded, and in accordance with specified time intervals
     if (fuseVelData || fusePosData || fuseHgtData) {
         // skip fusion as required to maintain ~dtVelPos time interval between corrections
@@ -1555,14 +1555,14 @@ void NavEKF::FuseVelPosNED()
         if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS;
         else gpsRetryTime = _gpsRetryTimeNoTAS;
 
-        // form the observation vector and zero observations if in static mode
+        // form the observation vector and zero velocity and horizontal position observations if in static mode
         if (~staticMode) {
             for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
             for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
-        observation[5] = -hgtMea;
         } else {
-            for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f;
+            for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
         }
+        observation[5] = -hgtMea;
 
         // calculate additional error in GPS velocity caused by manoeuvring
         NEvelErr = _gpsNEVelVarAccScale * accNavMag;