From 04944fa6cec474a1baf86bc8d5c6882c5c4f184a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Jul 2019 08:55:39 +1000 Subject: [PATCH] AP_NavEKF3: review fixes thanks Paul! --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 3 +++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 415bfd0b1b..42342b463d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -712,6 +712,9 @@ bool NavEKF3::InitialiseFilter(void) // Set the primary initially to be the lowest index primary = 0; + // invalidate shared origin + common_origin_valid = false; + // initialise the cores. We return success only if all cores // initialise successfully bool ret = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 461148780a..8716e49f7f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -481,7 +481,6 @@ bool NavEKF3_core::setOriginLLH(const Location &loc) // Set the NED origin to be used until the next filter reset void NavEKF3_core::setOrigin(const Location &loc) { - // assume origin at current GPS location (no averaging) EKF_origin = loc; // if flying, correct for height change from takeoff so that the origin is at field elevation if (inFlight) {