Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF2: added EK2_OPTIONS #27852

Merged
merged 1 commit into from
Aug 20, 2024
+20 −4
Merged

AP_NavEKF2: added EK2_OPTIONS #27852

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 9 additions & 4 deletions libraries/AP_NavEKF2/AP_NavEKF2.cpp
View file
Open in desktop
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
View file
Open in desktop
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
Loading