AP_NavEKF2: non_GPS modes ensure EKF origin set only once and stays in sync

ekf2

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1125,10 +1125,11 @@ bool NavEKF2::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if (_fusionModeGPS != 3) {
-        // we don't allow setting of the EKF origin unless we are
-        // flying in non-GPS mode. This is to prevent accidental set
-        // of EKF origin with invalid position or height
+    if (_fusionModeGPS != 3 || common_origin_valid) {
+        // we don't allow setting of the EKF origin if using GPS
+        // or if the EKF origin has already been set.
+        // This is to prevent accidental setting of EKF origin with an
+        // invalid position or height or causing upsets from a shifting origin.
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 refusing set origin");
         return false;
     }

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -422,23 +422,27 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
     return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
 }
 
-// set the LLH location of the filters NED origin
+// sets the local NED origin using a LLH location (latitude, longitude, height)
+// returns false if absolute aiding and GPS is being used or if the origin is already set
 bool NavEKF2_core::setOriginLLH(const Location &loc)
 {
     if (PV_AidingMode == AID_ABSOLUTE && !extNavUsedForPos) {
+        // reject attempts to set the origin if GPS is being used
         return false;
     }
-    EKF_origin = loc;
-    ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
-    // define Earth rotation vector in the NED navigation frame at the origin
-    calcEarthRateNED(earthRateNED, loc.lat);
-    validOrigin = true;
-    return true;
+
+    return setOrigin(loc);
 }
 
-// Set the NED origin to be used until the next filter reset
-void NavEKF2_core::setOrigin(const Location &loc)
+// sets the local NED origin using a LLH location (latitude, longitude, height)
+// returns false if the origin has already been set
+bool NavEKF2_core::setOrigin(const Location &loc)
 {
+    // if the origin is valid reject setting a new origin
+    if (validOrigin) {
+        return false;
+    }
+
     EKF_origin = loc;
     // if flying, correct for height change from takeoff so that the origin is at field elevation
     if (inFlight) {
@@ -453,6 +457,8 @@ void NavEKF2_core::setOrigin(const Location &loc)
     // 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;
+
+    return true;
 }
 
 // record a yaw reset event

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -584,7 +584,13 @@ void NavEKF2_core::readGpsData()
 
             // see if we can get origin from frontend
             if (!validOrigin && frontend->common_origin_valid) {
-                setOrigin(frontend->common_EKF_origin);
+
+                if (!setOrigin(frontend->common_EKF_origin)) {
+                    // set an error as an attempt was made to set the origin more than once
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                    return;
+                }
+
             }
 
             // Read the GPS location in WGS-84 lat,long,height coordinates
@@ -592,7 +598,12 @@ void NavEKF2_core::readGpsData()
 
             // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
             if (gpsGoodToAlign && !validOrigin) {
-                setOrigin(gpsloc);
+
+                if (!setOrigin(gpsloc)) {
+                    // set an error as an attempt was made to set the origin more than once
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                    return;
+                }
 
                 // 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

@@ -160,8 +160,7 @@ public:
 
     // set the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter will be relative to this location
-    // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
-    // Returns false if the filter has rejected the attempt to set the origin
+    // returns false if absolute aiding and GPS is being used or if the origin is already set
     bool setOriginLLH(const Location &loc);
 
     // return estimated height above ground level
@@ -669,8 +668,10 @@ private:
     // Control reset of yaw and magnetic field states
     void controlMagYawReset();
 
-    // Set the NED origin to be used until the next filter reset
-    void setOrigin(const Location &loc);
+    // set the latitude and longitude and height used to set the NED origin
+    // All NED positions calculated by the filter will be relative to this location
+    // returns false if the origin has already been set
+    bool setOrigin(const Location &loc);
 
     // Assess GPS data quality and set gpsGoodToAlign if good enough to align the EKF
     void calcGpsGoodToAlign(void);