From 2f3b2a7111d2e37b6f04e97db4a0412becbf5b9b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 5 Jan 2014 20:54:49 +1100
Subject: [PATCH] AP_NavEKF: second attempt at higher rate baro fusion

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 45 ++++++++-----------------------
 libraries/AP_NavEKF/AP_NavEKF.h   |  6 ++++-
 2 files changed, 16 insertions(+), 35 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index c9be64f5b4..10101c9e9a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -257,37 +257,17 @@ void NavEKF::SelectVelPosFusion()
     {
         // Command fusion of GPS measurements if new ones available
         readGpsData();
-        readHgtData();
         if (newDataGps)
         {
             fuseVelData = true;
             fusePosData = true;
-            fuseHgtData = true;
         }
-        // Don't wait longer than HGTmsecMax msec between height updates
-        // if no GPS
-        else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
+        readHgtData();
+        if (newDataHgt || ((IMUmsec - HGTmsecPrev) >= HGTmsecMax))
         {
-            // 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;
-            }
+            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
         if (fuseVelData || fusePosData || fuseHgtData)
@@ -1958,18 +1938,15 @@ void NavEKF::readGpsData()
 
 void NavEKF::readHgtData()
 {
-    // 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) {
-        newDataHgt == true;
-        hgtMeaPrev = hgtMea;
-    }
-    else {
-        newDataHgt == false;
+    if (_baro.get_last_update() != lastHgtUpdate) {
+        lastHgtUpdate = _baro.get_last_update();
+        hgtMea = _baro.get_altitude();
+        newDataHgt = true;
+        // recall states from compass measurement time
+        RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
+    } else {
+        newDataHgt = false;
     }
-    // recall states from compass measurement time
-    RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
 }
 
 void NavEKF::readMagData()
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 9859a877d0..cda74588a8 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -267,7 +267,7 @@ private:
     uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
     const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
     const bool fuseMeNow; // boolean to force fusion whenever data arrives
-    bool staticMode; // boolean to force positio and velocity measurements to zero for pre-arm or bench testing
+    bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing
 
     // last time compass was updated
     uint32_t lastMagUpdate;
@@ -297,6 +297,10 @@ private:
     // TAS input variables
     bool newDataTas;
 
+    // HGT input variables
+    bool newDataHgt;
+    uint32_t lastHgtUpdate;
+
     // Time stamp when vel, pos or height measurements last failed checks
 	uint32_t velFailTime;
 	uint32_t posFailTime;