diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index dbbac13a957a90..4f8006ccac945f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1089,6 +1089,10 @@ bool NavEKF2::getOriginLLH(Location &loc) const if (!core) { return false; } + if (common_origin_valid) { + loc = common_EKF_origin; + return true; + } return core[primary].getOriginLLH(loc); }