From 32cc427ff4d8406f3ebf7143421028ecebbe821f Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 5 Jan 2014 18:15:13 +1100
Subject: [PATCH] AP_NavEKF: Modifed Vel Pos fusion to fuse height data
 whenever baro reading has changed

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 40 ++++++++++---------------------
 libraries/AP_NavEKF/AP_NavEKF.h   |  6 ++++-
 2 files changed, 18 insertions(+), 28 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 1429753b67..8fc5358f5e 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -71,6 +71,8 @@ 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
@@ -262,31 +264,12 @@ void NavEKF::SelectVelPosFusion()
         {
             fuseVelData = true;
             fusePosData = true;
-            fuseHgtData = true;
         }
         // Don't wait longer than HGTmsecMax msec between height updates
         // if no GPS
-        else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
+        if (newDataHgt)
         {
-            // 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;
+            fuseHgtData = true;
         }
         // set flag to let other processes know that GPS and/or height fusion has
         // ocurred in this frame
@@ -294,6 +277,7 @@ void NavEKF::SelectVelPosFusion()
         {
             FuseVelPosNED();
             newDataGps = false;
+            newDataHgt = false;
             posVelFuseStep = true;
         }
         else
@@ -1958,14 +1942,16 @@ void NavEKF::readGpsData()
 
 void NavEKF::readHgtData()
 {
-    // ToDo do we check for new height data or grab a height measurement?
+    // ToDo - better check for new height data
     // Best to do this at the same time as GPS measurement fusion for efficiency
     hgtMea = _baro.get_altitude();
-#if 0
-    Vector3f pos;
-    getPosNED(pos);
-    //::printf("BARO.Alt=%.2f REL.z=%.2f\n", hgtMea, -pos.z);
-#endif
+    if (hgtMeaPrev != hgtMea) {
+        newDataHgt == true;
+        hgtMeaPrev = hgtMea;
+    }
+    else {
+        newDataHgt == false;
+    }
     // recall states from compass measurement time
     RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
 }
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 9859a877d0..0585f4f117 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^2
+    ftype _accNoise; // accelerometer process noise : m/s^2hgtHealth = (sq(hgtInnov) < maxf((100.0f * varInnovVelPos[5])) , 5.0f);
     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,6 +297,10 @@ 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;