From e2b8807260bb7e66ed84ae7941d1316095244435 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 17 Jul 2016 14:27:37 +1000 Subject: [PATCH] AP_NavEKF2: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index c38ea3e557..48130ad5a9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -313,7 +313,7 @@ bool NavEKF2_core::optFlowDataPresent(void) const // return true if the filter to be ready to use gps bool NavEKF2_core::readyToUseGPS(void) const { - return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3); + return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse; } // return true if we should use the compass diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 634be7b4a0..520d55d4ea 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -417,6 +417,8 @@ void NavEKF2_core::readGpsData() if (PV_AidingMode != AID_ABSOLUTE) { // Pre-alignment checks gpsGoodToAlign = calcGpsGoodToAlign(); + } else { + gpsGoodToAlign = false; } // Post-alignment checks