diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 338fc90439..e5d08635f2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -426,6 +426,10 @@ private: const uint8_t gndGradientSigma = 50; // RMS terrain gradient percentage assumed by the terrain height estimation const uint8_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec + // origin set by one of the cores + struct Location common_EKF_origin; + bool common_origin_valid; + struct { bool enabled:1; bool log_compass:1; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 145461023b..4b26313979 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -437,10 +437,10 @@ bool NavEKF2_core::setOriginLLH(const Location &loc) } // Set the NED origin to be used until the next filter reset -void NavEKF2_core::setOrigin() +void NavEKF2_core::setOrigin(const Location &loc) { // assume origin at current GPS location (no averaging) - EKF_origin = AP::gps().location(); + EKF_origin = loc; // if flying, correct for height change from takeoff so that the origin is at field elevation if (inFlight) { EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z); @@ -449,7 +449,11 @@ void NavEKF2_core::setOrigin() // define Earth rotation vector in the NED navigation frame at the origin calcEarthRateNED(earthRateNED, EKF_origin.lat); validOrigin = true; - gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin set to GPS",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u origin set",(unsigned)imu_index); + + // put origin in frontend as well to ensure it stays in sync between lanes + frontend->common_EKF_origin = EKF_origin; + frontend->common_origin_valid = true; } // record a yaw reset event diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index be8c8c352c..8d32446332 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -530,12 +530,17 @@ void NavEKF2_core::readGpsData() // Post-alignment checks calcGpsGoodForFlight(); + // see if we can get origin from frontend + if (!validOrigin && frontend->common_origin_valid) { + setOrigin(frontend->common_EKF_origin); + } + // Read the GPS location in WGS-84 lat,long,height coordinates const struct Location &gpsloc = gps.location(); // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed if (gpsGoodToAlign && !validOrigin) { - setOrigin(); + setOrigin(gpsloc); // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index dc1bae5d55..81bc7e1b54 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -703,7 +703,7 @@ private: void controlMagYawReset(); // Set the NED origin to be used until the next filter reset - void setOrigin(); + void setOrigin(const Location &loc); // determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect bool getTakeoffExpected();