From 2c012c2763bb26c30e988509020cca33e810d57c Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 28 Jan 2015 15:24:10 +1100 Subject: [PATCH] AP_NavEKF: Always check for new GPS data This fixes a bug that meant that once the EKF had started up in a non-GPS mode, it would no longer read the GPS and therefore would never be able to use GPS again until reset. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 16957d3806..861a499eec 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -737,11 +737,11 @@ void NavEKF::UpdateFilter() // select fusion of velocity, position and height measurements void NavEKF::SelectVelPosFusion() { - // check for new data, specify which measurements should be used and check data for freshness - if (PV_AidingMode == AID_ABSOLUTE) { + // check for and read new GPS data + readGpsData(); - // check for and read new GPS data - readGpsData(); + // Specify which measurements should be used and check data for freshness + if (PV_AidingMode == AID_ABSOLUTE) { // check if we can use opticalflow as a backup bool optFlowBackup = (flowDataValid && !hgtTimeout);