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.

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

AHRS/VisualOdom/Copter: allow visual odometry to align yaw to GPS-for-yaw #18380

Merged
merged 7 commits into from
Aug 30, 2021

Conversation

rmackay9
Copy link
Contributor

@rmackay9 rmackay9 commented Aug 18, 2021

This PR (in a roundabout way) allows AP_VisualOdom (aka ExternalNav) to align its yaw to the AHRS's yaw even if the yaw source is GSF or GPS-for-yaw. This resolves the 2nd item in issue #15859

To provide some background, in AP_VisualOdom_IntelT265::align_sensor_to_vehicle() we have the check shown below to protect against re-aligning the external nav system's yaw (e.g. T265) to the AHRS's yaw if the AHRS is using the camera as its source (aka "drinking our own bathwater"). This is important to avoid a slight random shift on each alignment.

// do not align to ahrs if it is using us as its yaw source
if (AP::ahrs().is_ext_nav_used_for_yaw()) {
    return false;
}

The problem is that the AP_AHRS::is_ext_nav_used_for_yaw() returns true (if EKF3 is active) if the yaw source is ExtNav, GSF or GPS-for-yaw meaning AP_VisualOdom can't align to any of these sources.. but it should be able to align to GSF or GPS-for-yaw. This badly named method is renamed to "using_noncompass_for_yaw" and a new "using_extnav_for_yaw" method is added.

This has been tested in SITL using gps-for-yaw and vicon together and below are screenshots showing BEFORE (where the visual odom could not be re-aligned to match gps-for-yaw) and AFTER where it was re-aligned.
sitl-before
sitl-test-aligned-yaw-to-gps-for-yaw

@tridge tridge merged commit e5bde1a into ArduPilot:master Aug 30, 2021
@rmackay9 rmackay9 deleted the ekf-extnav-yaw-fix branch August 31, 2021 00:47
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants