diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index d7fefdccd3..025d5e8d22 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -524,6 +524,10 @@ private: bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited + // origin set by one of the cores + struct Location common_EKF_origin; + bool common_origin_valid; + // update the yaw reset data to capture changes due to a lane switch // new_primary - index of the ekf instance that we are about to switch to as the primary // old_primary - index of the ekf instance that we are currently using as the primary diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index b3ed8464eb..461148780a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -479,10 +479,10 @@ bool NavEKF3_core::setOriginLLH(const Location &loc) } // Set the NED origin to be used until the next filter reset -void NavEKF3_core::setOrigin() +void NavEKF3_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); @@ -491,7 +491,11 @@ void NavEKF3_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, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF3 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_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 5cc3db3878..aa388e0c8d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -582,12 +582,17 @@ void NavEKF3_core::readGpsData() // Post-alignment checks calcGpsGoodForFlight(); + // see if we can get an origin from the 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_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 06aeb8d639..66f9217e91 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -768,7 +768,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();