Skip to content

Commit

Permalink
AP_NavEKF2: added EK2_OPTIONS
Browse files Browse the repository at this point in the history
this allows for disabling of external navigation data (such as visual
odomotry) in EKF2. This is very useful when using EKF2 as a fallback
and truth reference when testing visual odomotry on EKF3
  • Loading branch information
tridge committed Aug 20, 2024
1 parent 61bd5e9 commit 91f8f25
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 4 deletions.
13 changes: 9 additions & 4 deletions libraries/AP_NavEKF2/AP_NavEKF2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -598,6 +598,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GSF_RST_MAX", 57, NavEKF2, _gsfResetMaxCount, 2),

// @Param: OPTIONS
// @DisplayName: Optional EKF behaviour
// @Description: optional EKF2 behaviour. Disabling external navigation prevents use of external vision data in the EKF2 solution
// @Bitmask: 0:DisableExternalNavigation
// @User: Advanced
AP_GROUPINFO("OPTIONS", 58, NavEKF2, _options, 0),

AP_GROUPEND
};
Expand Down Expand Up @@ -1514,8 +1521,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);

if (core) {
if (!option_is_set(Option::DisableExternalNav) && core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
}
Expand Down Expand Up @@ -1568,8 +1574,7 @@ void NavEKF2::writeDefaultAirSpeed(float airspeed)
void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);

if (core) {
if (!option_is_set(Option::DisableExternalNav) && core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
}
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_NavEKF2/AP_NavEKF2.h
Original file line number Diff line number Diff line change
Expand Up @@ -364,7 +364,18 @@ class NavEKF2 {
AP_Int8 _gsfRunMask; // mask controlling which EKF2 instances run a separate EKF-GSF yaw estimator
AP_Int8 _gsfUseMask; // mask controlling which EKF2 instances will use EKF-GSF yaw estimator data to assit with yaw resets
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF2 is allowed to reset it's yaw to the EKF-GSF estimate
AP_Int32 _options; // optional behaviour bitmask

// enum for processing options
enum class Option {
DisableExternalNav = (1U<<0),
};

// return true if an option is set
bool option_is_set(Option option) const {
return (uint32_t(option) & uint32_t(_options)) != 0;
}

// Possible values for _flowUse
#define FLOW_USE_NONE 0
#define FLOW_USE_NAV 1
Expand Down

0 comments on commit 91f8f25

Please sign in to comment.