AP_NavEKF3: review fixes

thanks Paul!
This commit is contained in:
Andrew Tridgell 2019-07-29 08:55:39 +10:00
parent 35ddaa2d42
commit bd4fc6fad8
2 changed files with 3 additions and 1 deletions

View File

@ -700,6 +700,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;

View File

@ -484,7 +484,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) {