Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: option to align extnav to optflow pos estimate

Browse Source
This commit is contained in:
Randy Mackay 2024-03-27 14:22:19 +09:00 committed by Andrew Tridgell
parent 1249388f23
commit 5a8ed5fb51
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -135,7 +135,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
// @Param: _OPTIONS // @Param: _OPTIONS
// @DisplayName: EKF Source Options // @DisplayName: EKF Source Options
// @Description: EKF Source Options // @Description: EKF Source Options
// @Bitmask: 0:FuseAllVelocities // @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES), AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),
@ -257,11 +257,12 @@ void AP_NavEKF_Source::align_inactive_sources()
return; return;
} }
// consider aligning XY position: // consider aligning ExtNav XY position:
bool align_posxy = false; bool align_posxy = false;
if ((getPosXYSource() == SourceXY::GPS) || if ((getPosXYSource() == SourceXY::GPS) ||
(getPosXYSource() == SourceXY::BEACON)) { (getPosXYSource() == SourceXY::BEACON) ||
// only align position if active source is GPS or 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++) { for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].posxy == SourceXY::EXTNAV) { if (_source_set[i].posxy == SourceXY::EXTNAV) {
// ExtNav could potentially be used, so align it // ExtNav could potentially be used, so align it

6
libraries/AP_NavEKF/AP_NavEKF_Source.h
View File

@ -47,7 +47,8 @@ public:
// enum for OPTIONS parameter // enum for OPTIONS parameter
enum class SourceOptions { 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 // initialisation
@ -118,6 +119,9 @@ private:
AP_Enum<SourceYaw> yaw; // yaw source AP_Enum<SourceYaw> yaw; // yaw source
} _source_set[AP_NAKEKF_SOURCE_SET_MAX]; } _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 AP_Int16 _options; // source options bitmask
uint8_t active_source_set; // index of active source set uint8_t active_source_set; // index of active source set