From cbc14d1fa5e9ece28277ea501cf4caa6cb5ec89e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 Dec 2023 08:59:27 +1100
Subject: [PATCH] AP_ExternalAHRS: align origin with AHRS origin

automatically set origin to AHRS origin. This means if on boot
external AHRS is not the primary then it will use the origin from the
active backend, preventing a jump on change of backends
---
 libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
index f75d6ac1d4..8d98810111 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
@@ -27,6 +27,7 @@
 #include "AP_ExternalAHRS_MicroStrain7.h"
 
 #include <GCS_MAVLink/GCS.h>
+#include <AP_AHRS/AP_AHRS.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -242,6 +243,20 @@ void AP_ExternalAHRS::update(void)
     if (backend) {
         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;
+        }
+    }
 }
 
 // Get model/type name