From 1ee7f00192520bf2e17bf05af9bfc6ddecadda5b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Feb 2021 20:37:06 +0900 Subject: [PATCH] AP_VisualOdom: fix T265 criteria for aligning to AHRS --- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index d75926441c..d4b41470f4 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -135,8 +135,13 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, return false; } - // if ahrs's yaw is from the compass, wait until it has been initialised - if (!AP::ahrs().is_ext_nav_used_for_yaw() && !AP::ahrs().yaw_initialised()) { + // 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; + } + + // do not align until ahrs yaw initialised + if (!AP::ahrs().initialised() || !AP::ahrs().yaw_initialised()) { return false; }