From 41fce38b98dce83a8de3dab3376671e1fd038e1c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 30 May 2024 09:04:57 +1000 Subject: [PATCH] AP_NavEKF3: ensure EK3 does not read GPS when disabled --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 5 +++++ libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9e1eab32eb..5f2755d660 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -547,6 +547,11 @@ void NavEKF3_core::readGpsData() // check for new GPS data const auto &gps = dal.gps(); + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) { + // don't read GPS data when disabled in the current source set + return; + } + // limit update rate to avoid overflowing the FIFO buffer if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) { return; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 8fc369c392..c84e3e287c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -234,7 +234,8 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate if(validOrigin) { auto &gps = dal.gps(); - if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) { + if (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS + && gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const Location &gpsloc = gps.location(selected_gps); posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat(); @@ -348,7 +349,8 @@ bool NavEKF3_core::getLLH(Location &loc) const bool NavEKF3_core::getGPSLLH(Location &loc) const { const auto &gps = dal.gps(); - if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D)) { + if (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS && + gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) { loc = gps.location(selected_gps); return true; }