From e32a8556b2bce6526d3afb620c1a87a6154ed016 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 17 Oct 2019 16:51:55 +0900
Subject: [PATCH] AP_NavEKF2: suppress ekf fail-to-set-origin if core disabled

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 95982bdda8..5f693c4975 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -1104,6 +1104,9 @@ bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const
 // Returns false if the filter has rejected the attempt to set the origin
 bool NavEKF2::setOriginLLH(const Location &loc)
 {
+    if (!core) {
+        return false;
+    }
     if (_fusionModeGPS != 3) {
         // we don't allow setting of the EKF origin unless we are
         // flying in non-GPS mode. This is to prevent accidental set
@@ -1111,9 +1114,6 @@ bool NavEKF2::setOriginLLH(const Location &loc)
         gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 refusing set origin");
         return false;
     }
-    if (!core) {
-        return false;
-    }
     bool ret = false;
     for (uint8_t i=0; i<num_cores; i++) {
         ret |= core[i].setOriginLLH(loc);