Skip to content

Commit

Permalink
AP_NavEKF3: added two more EK3_OPTION bits
Browse files Browse the repository at this point in the history
these allow for easier GPS-denied testing of EKF3, and allows for
replay testing without the SetLatLng code being active.

Note that the gaps in the bitmask are for pending range beacon
features, we don't want to fill in the gaps so that existing replay
logs for that feature remain valid
  • Loading branch information
tridge committed Nov 27, 2024
1 parent 5ca1e48 commit 80d5db1
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 2 deletions.
16 changes: 14 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -738,8 +738,8 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {

// @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
// @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. The DisableSetLatLng option disables processing of SetLatLng commands provided by an external estimator. The DisableGPSAtStartup option allows for easier GPS denied testing to ensure that no GPS data is processed before an AUX function to disable GPS can be processed.
// @Bitmask: 0:JammingExpected,2:DisableSetLatLng,5:DisableGPSAtStartup
// @User: Advanced
AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0),

Expand Down Expand Up @@ -787,6 +787,14 @@ bool NavEKF3::InitialiseFilter(void)
}
#endif

if (option_is_enabled(Options::DisableGPSAtStartup)) {
// this is equivalent to the auxillary function for GPS
// disable being active at startup. Useful for GPS-denied
// testing to ensure no use is made of the GPS while still
// allowing GPS on EKF2
_gps_disabled = true;
}

if (core == nullptr) {

// don't run multiple filters for 1 IMU
Expand Down Expand Up @@ -1421,6 +1429,10 @@ bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timesta
#if EK3_FEATURE_POSITION_RESET
dal.log_SetLatLng(loc, posAccuracy, timestamp_ms);

if (option_is_enabled(Options::DisableSetLatLng)) {
return false;
}

if (!core) {
return false;
}
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3.h
Original file line number Diff line number Diff line change
Expand Up @@ -455,6 +455,8 @@ class NavEKF3 {
// enum for processing options
enum class Option {
JammingExpected = (1<<0),
DisableSetLatLng = (1<<2),
DisableGPSAtStartup = (1<<5),
};
bool option_is_enabled(Option option) const {
return (_options & (uint32_t)option) != 0;
Expand Down

0 comments on commit 80d5db1

Please sign in to comment.