AP_NavEKF: second attempt at higher rate baro fusion

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
+        readHgtData();
-        // if no GPS
+        if (newDataHgt || ((IMUmsec - HGTmsecPrev) >= HGTmsecMax))
-        else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
         {
-            // Static mode is used for pre-arm and bench testing and allows operation
+            fuseHgtData = true;
-            // 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)
@@ -1958,18 +1938,15 @@ void NavEKF::readGpsData()
 
 void NavEKF::readHgtData()
 {
-    // ToDo do we check for new height data or grab a height measurement?
+    if (_baro.get_last_update() != lastHgtUpdate) {
-    // Best to do this at the same time as GPS measurement fusion for efficiency
+        lastHgtUpdate = _baro.get_last_update();
-    hgtMea = _baro.get_altitude();
+        hgtMea = _baro.get_altitude();
-    if (hgtMeaPrev != hgtMea) {
+        newDataHgt = true;
-        newDataHgt == true;
+        // recall states from compass measurement time
-        hgtMeaPrev = hgtMea;
+        RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
-    }
+    } else {
-    else {
+        newDataHgt = false;
-        newDataHgt == false;
     }
-    // recall states from compass measurement time
-    RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
 }
 
 void NavEKF::readMagData()

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;