From d7ecd6883d776deeecdbbb929d50680312b53d1a Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Wed, 16 Jun 2021 14:13:48 -0400 Subject: [PATCH] AP_NavEKF2: non_GPS modes ensure EKF origin set only once and stays in sync ekf2 --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 9 +++---- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 24 ++++++++++++------- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 15 ++++++++++-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 9 +++---- 4 files changed, 38 insertions(+), 19 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index e2ca88245e..b5d5431b89 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 5307d00aae..f89cef8b05 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index efd7b81b5b..3563606777 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 6d79791ba4..9fed911eec 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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);