diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 26bbeffc23d5e8..387f95781fd32b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1392,11 +1392,9 @@ bool NavEKF3::setOriginLLH(const Location &loc) if (!core) { return false; } - 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. + if (common_origin_valid) { + // we don't allow setting the EKF origin if it has already been set + // this is to prevent causing upsets from a shifting origin. GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin"); return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 33a796b2ba6c76..c886f48ab6328e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -652,14 +652,9 @@ bool NavEKF3_core::assume_zero_sideslip(void) const } // 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 +// returns false 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 attempts to set the origin if GPS is being used or if the origin is already set - return false; - } - return setOrigin(loc); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2b5138062ffaed..eef68881dd4a63 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -224,7 +224,7 @@ class NavEKF3_core : public NavEKF_core_common // 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 Absolute aiding and GPS is being used or if the origin is already set + // returns false if the origin has already been set bool setOriginLLH(const Location &loc); // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty