From ddb7d92fc8a49b3ed43e87c5d3692d4fdc100ed5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 26 Nov 2015 09:21:48 +1100 Subject: [PATCH] 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 | 8 ++------ libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 10 ++++++---- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 3 +-- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index a926ea5ea2..5321912128 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/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 (tasDataWaiting) - { + if (tasDataToFuse && statesInitialised && !inhibitWindStates) { FuseAirspeed(); prevTasStep_ms = imuSampleTime_ms; - tasDataWaiting = false; - newDataTas = false; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 547756bd7a..4245c5b8ca 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 2ff2252c41..ea23c4d6c7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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 tasDataWaiting; // true when new airspeed data is waiting to be fused + bool tasDataToFuse; // 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)