From beaaa5a7f544f881fcfc0bd1a67cace2cea4ec99 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 27 Jul 2019 15:29:15 +1000
Subject: [PATCH] AP_NavEKF2: set origin on all cores

when EKF origin is set, make sure it is set on all cores
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 900519a12b..1e835fa23c 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -1034,7 +1034,12 @@ bool NavEKF2::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    return core[primary].setOriginLLH(loc);
+    bool ret = false;
+    for (uint8_t i=0; i<num_cores; i++) {
+        ret |= core[i].setOriginLLH(loc);
+    }
+    // return true if any core accepts the new origin
+    return ret;
 }
 
 // return estimated height above ground level