Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Rework GPS jamming resiliency

Browse Source 
Make it user selectable.
Remove potential for a race condition between decisions based on latest data and the EKF fusion processing which operates on a delayed time horizon. This is achieved by preventing data entering the buffer if awaiting checks to pass ensuring that no EKF fusion time horizon processes can use data that hasn't passed checks.
Log the waitingForGpsChecks class variable
This commit is contained in:
Paul Riseborough 2023-09-27 15:35:31 +10:00 committed by Andrew Tridgell
parent 996bf7d4df
commit 720c2da807
6 changed files with 43 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -734,6 +734,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @Units: m
AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f),
// @Param: OPTIONS
// @DisplayName: Optional EKF behaviour
// @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad
// @Bitmask: 0:JammingExpected
// @User: Advanced
AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0),
AP_GROUPEND
};

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -448,6 +448,12 @@ private:
AP_Int8 _primary_core; // initial core number
AP_Enum<LogLevel> _log_level; // log verbosity level
AP_Float _gpsVAccThreshold; // vertical accuracy threshold to use GPS as an altitude source
AP_Int32 _options; // bit mask of processing options
// enum for processing options
enum class Options {
JammingExpected = (1<<0),
};
// Possible values for _flowUse
#define FLOW_USE_NONE 0
@ -487,6 +493,7 @@ private:
const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec)
const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
const uint16_t deadReckonDeclare_ms = 1000; // Time without equivalent position or velocity observation to constrain drift before dead reckoning is declared (msec)
const uint16_t gpsNoFixTimeout_ms = 2000; // Time without a fix required to reset GPS alignment checks when EK3_OPTIONS bit 0 is set (msec)
// time at start of current filter update
uint64_t imuSampleTime_us;

3
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -101,7 +101,8 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
gps_index : selected_gps,
airspeed_index : getActiveAirspeed(),
source_set : frontend->sources.getPosVelYawSourceSet(),
gps_good_to_align : gpsGoodToAlign
gps_good_to_align : gpsGoodToAlign,
wait_for_gps_checks : waitingForGpsChecks
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

42
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -542,24 +542,6 @@ void NavEKF3_core::readGpsData()
// check for new GPS data
const auto &gps = dal.gps();
if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
// The GPS has dropped lock so force quality checks to restart
gpsGoodToAlign = false;
lastGpsVelFail_ms = imuSampleTime_ms;
lastGpsVelPass_ms = 0;
const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
const bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip());
const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingBodyVelNav);
if (canDeadReckon) {
// If we can do dead reckoning with a data source other than GPS there is time to wait
// for GPS alignment checks to pass before using GPS inside the EKF.
waitingForGpsChecks = true;
} else {
waitingForGpsChecks = false;
}
}
// limit update rate to avoid overflowing the FIFO buffer
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
return;
@ -651,6 +633,27 @@ void NavEKF3_core::readGpsData()
useGpsVertVel = false;
}
if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) &&
(lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) {
const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
const bool canDoWindRelNav = assume_zero_sideslip();
const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav);
if (canDeadReckon) {
// If we can do dead reckoning with a data source other than GPS there is time to wait
// for GPS alignment checks to pass before using GPS inside the EKF.
// this gets set back to false in calcGpsGoodToAlign() when GPS checks pass
waitingForGpsChecks = true;
// force GPS checks to restart
lastPreAlignGpsCheckTime_ms = 0;
lastGpsVelFail_ms = imuSampleTime_ms;
lastGpsVelPass_ms = 0;
gpsGoodToAlign = false;
} else {
waitingForGpsChecks = false;
}
}
// Monitor quality of the GPS velocity data before and after alignment
calcGpsGoodToAlign();
@ -699,7 +702,8 @@ void NavEKF3_core::readGpsData()
}
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {
// and are not waiting for GPs checks to pass
if (validOrigin && !waitingForGpsChecks) {
gpsDataNew.lat = gpsloc.lat;
gpsDataNew.lng = gpsloc.lng;
if ((frontend->_originHgtMode & (1<<2)) == 0) {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1038,7 +1038,7 @@ private:
bool dragTimeout; // boolean true if drag measurements have failed for too long and have timed out
bool badIMUdata; // boolean true if the bad IMU data is detected
bool velAiding; // boolean true if the velocity drift is constrained by observations
bool waitingForGpsChecks; // boolean true if the EKF should not retrieve GPS data from the buffer until quality checks have passed
bool waitingForGpsChecks; // boolean true if the EKF should write GPS data to the buffer until quality checks have passed
uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected
uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected
uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit

4
libraries/AP_NavEKF3/LogStructure.h
View File

@ -345,6 +345,7 @@ struct PACKED log_XKQ {
// @Field: AI: airspeed selection index
// @Field: SS: Source Set (primary=0/secondary=1/tertiary=2)
// @Field: GPS_GTA: GPS good to align
// @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass
struct PACKED log_XKFS {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -355,6 +356,7 @@ struct PACKED log_XKFS {
uint8_t airspeed_index;
uint8_t source_set;
uint8_t gps_good_to_align;
uint8_t wait_for_gps_checks;
};
// @LoggerMessage: XKTV
@ -441,7 +443,7 @@ struct PACKED log_XKV {
{ LOG_XKFM_MSG, sizeof(log_XKFM), \
"XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \
{ LOG_XKFS_MSG, sizeof(log_XKFS), \
"XKFS","QBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA", "s#------", "F-------" , true }, \
"XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \
{ LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \
{ LOG_XKT_MSG, sizeof(log_XKT), \
"XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \