mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: non_GPS modes ensure EKF origin set only once and stays in sync
ekf3
This commit is contained in:
parent
d7ecd6883d
commit
0ae3730f11
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue