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_NavEKF: option to align extnav to optflow pos estimate #27487

Merged
merged 1 commit into from
Jul 9, 2024
Merged

AP_NavEKF: option to align extnav to optflow pos estimate #27487

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
9 changes: 5 additions & 4 deletions libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
// @Param: _OPTIONS
// @DisplayName: EKF Source Options
// @Description: EKF Source Options
// @Bitmask: 0:FuseAllVelocities
// @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow
// @User: Advanced
AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),

Expand Down Expand Up @@ -257,11 +257,12 @@ void AP_NavEKF_Source::align_inactive_sources()
return;
}

// consider aligning XY position:
// consider aligning ExtNav XY position:
bool align_posxy = false;
if ((getPosXYSource() == SourceXY::GPS) ||
(getPosXYSource() == SourceXY::BEACON)) {
// only align position if active source is GPS or Beacon
(getPosXYSource() == SourceXY::BEACON) ||
((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
// align ExtNav position if active source is GPS, Beacon or (optionally) Optflow
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].posxy == SourceXY::EXTNAV) {
// ExtNav could potentially be used, so align it
Expand Down
6 changes: 5 additions & 1 deletion libraries/AP_NavEKF/AP_NavEKF_Source.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@ class AP_NavEKF_Source

// enum for OPTIONS parameter
enum class SourceOptions {
FUSE_ALL_VELOCITIES = (1 << 0) // fuse all velocities configured in source sets
FUSE_ALL_VELOCITIES = (1 << 0), // fuse all velocities configured in source sets
ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1) // align position of inactive sources to ahrs when using optical flow
};

// initialisation
Expand Down Expand Up @@ -118,6 +119,9 @@ class AP_NavEKF_Source
AP_Enum<SourceYaw> yaw; // yaw source
} _source_set[AP_NAKEKF_SOURCE_SET_MAX];

// helper to check if an option parameter bit has been set
bool option_is_set(SourceOptions option) const { return (_options.get() & int16_t(option)) != 0; }

AP_Int16 _options; // source options bitmask

uint8_t active_source_set; // index of active source set
Expand Down
Loading