AP_NavEKF3: ensure EK3 does not read GPS when disabled

tridge · May 29, 2024 · 7f42611 · 7f42611
1 parent e27dea7
commit 7f42611
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -542,6 +542,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
@@ -230,7 +230,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();
@@ -344,7 +345,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;
     }