From f2acad4099004306d81a2ba63ee838593ae86948 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Mon, 24 May 2021 11:11:56 +0900 Subject: [PATCH] AP_VisualOdom: use get_yaw instead of to_axis_angle --- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index d4b41470f4..95c617f478 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -129,12 +129,6 @@ 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) { - // fail immediately if ahrs cannot provide attitude - Quaternion ahrs_quat; - if (!AP::ahrs().get_quaternion(ahrs_quat)) { - return false; - } - // 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; @@ -165,10 +159,8 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const float sens_yaw = att_corrected.get_euler_yaw(); // trim yaw by difference between ahrs and sensor yaw - Vector3f angle_diff; - ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff); const float yaw_trim_orig = _yaw_trim; - _yaw_trim = angle_diff.z; + _yaw_trim = wrap_2PI(AP::ahrs().get_yaw() - sens_yaw); gcs().send_text(MAV_SEVERITY_CRITICAL, "VisOdom: yaw shifted %d to %d deg", (int)degrees(_yaw_trim - yaw_trim_orig), (int)wrap_360(degrees(sens_yaw + _yaw_trim)));