From da2c341914a67504e253b612dc88f53f32b59ab4 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 5 Jan 2014 20:02:07 +1100
Subject: [PATCH] Revert "AP_NavEKF: Modifed Vel Pos fusion to fuse height data
 whenever baro reading has changed"

This reverts commit d1e1be192a0e7ab0771edc9f2c03547da25697cf.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 28 ++++++++++++++++++++++------
 libraries/AP_NavEKF/AP_NavEKF.h   |  6 +-----
 2 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 8fc5358f5e..df1a576390 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -71,8 +71,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     hgtRate = 0.0f;
     mag_state.q0 = 1;
     mag_state.DCM.identity();
-    hgtMeaPrev = 0.0f;
-    hgtMea = 0.0f;
 }
 
 bool NavEKF::healthy(void) const
@@ -264,12 +262,31 @@ void NavEKF::SelectVelPosFusion()
         {
             fuseVelData = true;
             fusePosData = true;
+            fuseHgtData = true;
         }
         // Don't wait longer than HGTmsecMax msec between height updates
         // if no GPS
-        if (newDataHgt)
+        else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
         {
-            fuseHgtData = true;
+            // Static mode is used for pre-arm and bench testing and allows operation
+            // without GPS
+            if (staticMode) {
+                fuseVelData = true;
+                fusePosData = true;
+                fuseHgtData = true;
+            }
+            else {
+                fuseVelData = false;
+                fusePosData = false;
+                fuseHgtData = true;
+            }
+            HGTmsecPrev = IMUmsec;
+        }
+        else
+        {
+            fuseVelData = false;
+            fusePosData = false;
+            fuseHgtData = false;
         }
         // set flag to let other processes know that GPS and/or height fusion has
         // ocurred in this frame
@@ -277,7 +294,6 @@ void NavEKF::SelectVelPosFusion()
         {
             FuseVelPosNED();
             newDataGps = false;
-            newDataHgt = false;
             posVelFuseStep = true;
         }
         else
@@ -1942,7 +1958,7 @@ void NavEKF::readGpsData()
 
 void NavEKF::readHgtData()
 {
-    // ToDo - better check for new height data
+    // ToDo do we check for new height data or grab a height measurement?
     // Best to do this at the same time as GPS measurement fusion for efficiency
     hgtMea = _baro.get_altitude();
     if (hgtMeaPrev != hgtMea) {
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 0585f4f117..9859a877d0 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -201,7 +201,7 @@ private:
     ftype _windStateNoise; // rate of change of wind : m/s^2
     ftype _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate
     ftype _gyrNoise; // gyro process noise : rad/s
-    ftype _accNoise; // accelerometer process noise : m/s^2hgtHealth = (sq(hgtInnov) < maxf((100.0f * varInnovVelPos[5])) , 5.0f);
+    ftype _accNoise; // accelerometer process noise : m/s^2
     ftype _dAngBiasNoise; // gyro bias state noise : rad/s^2
     ftype _magEarthNoise; // earth magnetic field process noise : gauss/sec
     ftype _magBodyNoise; // earth magnetic field process noise : gauss/sec
@@ -297,10 +297,6 @@ private:
     // TAS input variables
     bool newDataTas;
 
-    // HGT input variables
-    float hgtMeaPrev;
-    bool newDataHgt;
-
     // Time stamp when vel, pos or height measurements last failed checks
 	uint32_t velFailTime;
 	uint32_t posFailTime;