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

ekf3

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1358,9 +1358,11 @@ bool NavEKF3::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if (sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) {
-        // we don't allow setting of the EKF origin if using GPS to prevent
-        // accidental setting of EKF origin with invalid position or height
+    if ((sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) || 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, "EKF3 refusing set origin");
         return false;
     }

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -585,25 +585,27 @@ bool NavEKF3_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 NavEKF3_core::setOriginLLH(const Location &loc)
 {
     if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
-        // reject attempt to set origin if GPS is being used
+        // reject attempts to set the origin if GPS is being used or if the origin is already set
         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 NavEKF3_core::setOrigin(const Location &loc)
+// sets the local NED origin using a LLH location (latitude, longitude, height)
+// returns false is the origin has already been set
+bool NavEKF3_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) {
@@ -618,6 +620,8 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -646,7 +646,12 @@ void NavEKF3_core::readGpsData()
 
             // see if we can get an origin from the 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
@@ -654,7 +659,12 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h

@@ -176,8 +176,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
@@ -806,8 +805,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
     void calcGpsGoodToAlign(void);