AP_NavEKF2: Fix timing jitter in airspeed fusion

The airspeed observation buffer was only being checked when new data arrived instead of every frame which introduced some timing jitter. The buffer is now checked every filer update step.
The duplication and inconsistent naming of booleans used to indicate availability f data has been fixed.

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -195,7 +195,7 @@ void NavEKF2_core::SelectTasFusion()
 {
     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     // If so, don't fuse measurements on this time step to reduce frame over-runs
-    // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
+    // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements
     if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) {
         airSpdFusionDelayed = true;
         return;
@@ -212,13 +212,9 @@ void NavEKF2_core::SelectTasFusion()
     }
 
     // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
-    tasDataWaiting = (statesInitialised && !inhibitWindStates && newDataTas);
+    if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
-    if (tasDataWaiting)
-    {
         FuseAirspeed();
         prevTasStep_ms = imuSampleTime_ms;
-        tasDataWaiting = false;
-        newDataTas = false;
     }
 }
 

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -544,6 +544,7 @@ void NavEKF2_core::calcFiltBaroOffset()
     // Apply a first order LPF with spike protection
     baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
 }
+
 /********************************************************
 *                Air Speed Measurements                 *
 ********************************************************/
@@ -561,14 +562,15 @@ void NavEKF2_core::readAirSpdData()
         tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
         timeTasReceived_ms = aspeed->last_update_ms();
         tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
+
         // Correct for the average intersampling delay due to the filter update rate
         tasDataNew.time_ms -= localFilterTimeStep_ms/2;
-        newDataTas = true;
+
+        // Save data into the buffer to be fused when the fusion time horizon catches up with it
         storedTAS.push(tasDataNew);
-        storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
-    } else {
-        newDataTas = false;
     }
+    // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
+    tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
 }
 
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -679,8 +679,7 @@ private:
     Vector3f velDotNED;             // rate of change of velocity in NED frame
     Vector3f velDotNEDfilt;         // low pass filtered velDotNED
     uint32_t imuSampleTime_ms;      // time that the last IMU value was taken
-    bool newDataTas;                // true when new airspeed data has arrived
+    bool tasDataToFuse;             // true when new airspeed data is waiting to be fused
-    bool tasDataWaiting;            // true when new airspeed data is waiting to be fused
     uint32_t lastBaroReceived_ms;   // time last time we received baro height data
     uint16_t hgtRetryTime_ms;       // time allowed without use of height measurements before a height timeout is declared
     uint32_t lastVelPassTime_ms;    // time stamp when GPS velocity measurement last passed innovation consistency check (msec)