Browse Source

AP_NavEKF3: review fixes

thanks Paul!
master
Andrew Tridgell 6 years ago
parent
commit
04944fa6ce
  1. 3
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 1
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

3
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -712,6 +712,9 @@ bool NavEKF3::InitialiseFilter(void)
// Set the primary initially to be the lowest index // Set the primary initially to be the lowest index
primary = 0; primary = 0;
// invalidate shared origin
common_origin_valid = false;
// initialise the cores. We return success only if all cores // initialise the cores. We return success only if all cores
// initialise successfully // initialise successfully
bool ret = true; bool ret = true;

1
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 // Set the NED origin to be used until the next filter reset
void NavEKF3_core::setOrigin(const Location &loc) void NavEKF3_core::setOrigin(const Location &loc)
{ {
// assume origin at current GPS location (no averaging)
EKF_origin = loc; EKF_origin = loc;
// if flying, correct for height change from takeoff so that the origin is at field elevation // if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) { if (inFlight) {

Loading…
Cancel
Save