From 2fddb64cf94a731028b020db5d442d7a49338eaa Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 16 Aug 2024 07:35:15 +1000
Subject: [PATCH] AP_AHRS: added common origin logic

this aligns the origin between EKF2, EKF3 and ExternalAHRS, making for
smooth transitions between AHRS backends in flight
---
 libraries/AP_AHRS/AP_AHRS.cpp | 52 ++++++++++++++++++++++++++++++++++-
 libraries/AP_AHRS/AP_AHRS.h   |  3 ++
 2 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index c87382d5d321f2..7eba96c71e5ed2 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -616,6 +616,22 @@ void AP_AHRS::update_EKF2(void)
             EKF2.getFilterStatus(filt_state);
             update_notify_from_filter_status(filt_state);
         }
+
+        /*
+          if we now have an origin then set in all backends
+        */
+        if (!done_common_origin) {
+            Location new_origin;
+            if (EKF2.getOriginLLH(new_origin)) {
+                done_common_origin = true;
+#if HAL_NAVEKF3_AVAILABLE
+                EKF3.setOriginLLH(new_origin);
+#endif
+#if AP_AHRS_EXTERNAL_ENABLED
+                external.set_origin(new_origin);
+#endif
+            }
+        }
     }
 }
 #endif
@@ -685,6 +701,21 @@ void AP_AHRS::update_EKF3(void)
             EKF3.getFilterStatus(filt_state);
             update_notify_from_filter_status(filt_state);
         }
+        /*
+          if we now have an origin then set in all backends
+        */
+        if (!done_common_origin) {
+            Location new_origin;
+            if (EKF3.getOriginLLH(new_origin)) {
+                done_common_origin = true;
+#if HAL_NAVEKF2_AVAILABLE
+                EKF2.setOriginLLH(new_origin);
+#endif
+#if AP_AHRS_EXTERNAL_ENABLED
+                external.set_origin(new_origin);
+#endif
+            }
+        }
     }
 }
 #endif
@@ -698,6 +729,22 @@ void AP_AHRS::update_external(void)
     if (_active_EKF_type() == EKFType::EXTERNAL) {
         copy_estimates_from_backend_estimates(external_estimates);
     }
+
+    /*
+      if we now have an origin then set in all backends
+    */
+    if (!done_common_origin) {
+        Location new_origin;
+        if (external.get_origin(new_origin)) {
+            done_common_origin = true;
+#if HAL_NAVEKF2_AVAILABLE
+            EKF2.setOriginLLH(new_origin);
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+            EKF3.setOriginLLH(new_origin);
+#endif
+        }
+    }
 }
 #endif // AP_AHRS_EXTERNAL_ENABLED
 
@@ -1411,6 +1458,9 @@ bool AP_AHRS::set_origin(const Location &loc)
 #if HAL_NAVEKF3_AVAILABLE
     const bool ret3 = EKF3.setOriginLLH(loc);
 #endif
+#if AP_AHRS_EXTERNAL_ENABLED
+    const bool ret_ext = external.set_origin(loc);
+#endif
 
     // return success if active EKF's origin was set
     bool success = false;
@@ -1440,7 +1490,7 @@ bool AP_AHRS::set_origin(const Location &loc)
 #endif
 #if AP_AHRS_EXTERNAL_ENABLED
     case EKFType::EXTERNAL:
-        // don't allow origin set with external AHRS
+        success = ret_ext;
         break;
 #endif
     }
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 16ec1009e03bf1..60ef391267dc7d 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -1018,6 +1018,9 @@ class AP_AHRS {
     bool option_set(Options option) const {
         return (_options & uint16_t(option)) != 0;
     }
+
+    // true when we have completed the common origin setup
+    bool done_common_origin;
 };
 
 namespace AP {