From 440fb8b03b1ea8edd8dfd26b01f4ccb7748a0410 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Mar 2024 14:43:30 +0900 Subject: [PATCH] AP_DAL: VisualOdom request_align_yaw_to_ahrs --- libraries/AP_DAL/AP_DAL_VisualOdom.cpp | 9 +++++++++ libraries/AP_DAL/AP_DAL_VisualOdom.h | 3 +++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.cpp b/libraries/AP_DAL/AP_DAL_VisualOdom.cpp index 503017a12b82e..1bb84eb91b6f4 100644 --- a/libraries/AP_DAL/AP_DAL_VisualOdom.cpp +++ b/libraries/AP_DAL/AP_DAL_VisualOdom.cpp @@ -8,6 +8,15 @@ #include "AP_DAL.h" #include +// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude +void AP_DAL_VisualOdom::request_align_yaw_to_ahrs() +{ +#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) + auto *vo = AP::visualodom(); + vo->request_align_yaw_to_ahrs(); +#endif +} + /* update position offsets to align to AHRS position should only be called when this library is not being used as the position source diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.h b/libraries/AP_DAL/AP_DAL_VisualOdom.h index 04ba7d5dd7273..3897c4e49943a 100644 --- a/libraries/AP_DAL/AP_DAL_VisualOdom.h +++ b/libraries/AP_DAL/AP_DAL_VisualOdom.h @@ -27,6 +27,9 @@ class AP_DAL_VisualOdom { return RVOH.pos_offset; } + // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude + void request_align_yaw_to_ahrs(); + // update position offsets to align to AHRS position // should only be called when this library is not being used as the position source void align_position_to_ahrs(bool align_xy, bool align_z);