From 4e2dd7c39921b99f6c5abcdc5142d81ae73abd3b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 28 Nov 2023 15:03:33 +0900
Subject: [PATCH] AP_NavEKF3: accept set origin even when using GPS

---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp         | 8 +++-----
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 7 +------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h      | 2 +-
 3 files changed, 5 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 26bbeffc23d..387f95781fd 100644
--- 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
index 33a796b2ba6..c886f48ab63 100644
--- 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
index 2b5138062ff..eef68881dd4 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -224,7 +224,7 @@ public:
 
     // 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