AP_NavEKF2: ensure that EKF origin stays in sync on all cores

this prevents the EKF origin on different cores from being initialised
to different values. A common value is stored in the frontend and used
by a core if it doesn't have an origin

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;

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

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

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();