From b723966b8d687738502139fa273bc97aee3c6142 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 19 Dec 2016 18:21:32 +1100 Subject: [PATCH] AP_NavEKF2: Add protection against bad GPS time delay values Prevent bad values for GPS time delay pushing the GPS time stamp outside the range of IMU data contained in the buffer. If this occurs it can prevent the GPS measurements from being fused and cause loss of navigation. --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 8f0c22c8f0..5b77bf89ad 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -417,8 +417,8 @@ void NavEKF3_core::readGpsData() // Correct for the average intersampling delay due to the filter updaterate gpsDataNew.time_ms -= localFilterTimeStep_ms/2; - // Prevent time delay exceeding age of oldest IMU data in the buffer - gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms); + // Prevent the time stamp falling outside the oldest and newest IMU data in the buffer + gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms); // Get which GPS we are using for position information gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor();