From e5bde1a085df9e9df8afc817ba32aab7137c1a10 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 18 Aug 2021 16:45:49 +0900 Subject: [PATCH] AP_VisualOdom: use AP_AHRS::using_extnav_for_yaw to protect against aligning to oneself --- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 6ddb0d901c..63eb7ecf59 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -129,8 +129,8 @@ void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const // use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude) { - // do not align to ahrs if it is using us as its yaw source - if (AP::ahrs().using_noncompass_for_yaw()) { + // do not align to ahrs if we are its yaw source + if (AP::ahrs().using_extnav_for_yaw()) { return false; }