From 91a5cfea6bc9b1f886aa23b3a6346617c08a0108 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 16 Aug 2024 07:33:54 +1000
Subject: [PATCH] AP_ExternalAHRS: added set_origin() call

and remove code that aligns origin to AHRS, this will be handled by
the AHRS common origin logic
---
 libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 23 +++++++++----------
 libraries/AP_ExternalAHRS/AP_ExternalAHRS.h   |  1 +
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
index 229ca00dea..9abbfff0d1 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
@@ -180,6 +180,17 @@ bool AP_ExternalAHRS::get_origin(Location &loc)
     return false;
 }
 
+bool AP_ExternalAHRS::set_origin(const Location &loc)
+{
+    WITH_SEMAPHORE(state.sem);
+    if (state.have_origin) {
+        return false;
+    }
+    state.origin = loc;
+    state.have_origin = true;
+    return true;
+}
+
 bool AP_ExternalAHRS::get_location(Location &loc)
 {
     if (!state.have_location) {
@@ -312,19 +323,7 @@ void AP_ExternalAHRS::update(void)
         backend->update();
     }
 
-    /*
-      if backend has not supplied an origin and AHRS has an origin
-      then use that origin so we get a common origin for minimum
-      disturbance when switching backends
-     */
     WITH_SEMAPHORE(state.sem);
-    if (!state.have_origin) {
-        Location origin;
-        if (AP::ahrs().get_origin(origin)) {
-            state.origin = origin;
-            state.have_origin = true;
-        }
-    }
 #if HAL_LOGGING_ENABLED
     const uint32_t now_ms = AP_HAL::millis();
     if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
index a08c282291..22ef0e9d5f 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
@@ -109,6 +109,7 @@ public:
     bool initialised(void) const;
     bool get_quaternion(Quaternion &quat);
     bool get_origin(Location &loc);
+    bool set_origin(const Location &loc);
     bool get_location(Location &loc);
     Vector2f get_groundspeed_vector();
     bool get_velocity_NED(Vector3f &vel);