AP_NavEKF3: accept set origin even when using GPS

ArduPilot · May 20, 2024 · c60eb06 · c60eb06
1 parent 3e868be
commit c60eb06
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 12 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -1392,11 +1392,9 @@ bool NavEKF3::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if ((sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) || common_origin_valid) {
-        // we don't allow setting of the EKF origin if using GPS
-        // or if the EKF origin has already been set.
-        // This is to prevent accidental setting of EKF origin with an
-        // invalid position or height or causing upsets from a shifting origin.
+    if (common_origin_valid) {
+        // we don't allow setting the EKF origin if it has already been set
+        // this is to prevent causing upsets from a shifting origin.
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
         return false;
     }

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -652,14 +652,9 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
 }
 
 // sets the local NED origin using a LLH location (latitude, longitude, height)
-// returns false if absolute aiding and GPS is being used or if the origin is already set
+// returns false if the origin is already set
 bool NavEKF3_core::setOriginLLH(const Location &loc)
 {
-    if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
-        // reject attempts to set the origin if GPS is being used or if the origin is already set
-        return false;
-    }
-
     return setOrigin(loc);
 }
 

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -224,7 +224,7 @@ class NavEKF3_core : public NavEKF_core_common
 
     // set the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter will be relative to this location
-    // returns false if Absolute aiding and GPS is being used or if the origin is already set
+    // returns false if the origin has already been set
     bool setOriginLLH(const Location &loc);
 
     // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty