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

@@ -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,20 +262,38 @@ 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)
         {
+            // 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
         if (fuseVelData || fusePosData || fuseHgtData)
         {
             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) {

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;