diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9e1eab32eb29d..5f2755d660412 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 8fc369c392f7d..c84e3e287c067 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; }