From e78336d0e5f9fb40752216fee9db10de05664f01 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Nov 2024 14:23:08 +0900 Subject: [PATCH] AP_VisualOdom: reduce yaw alignment spamming --- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index b1f65d977f51e5..648626185197fe 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -200,9 +200,13 @@ void AP_VisualOdom_IntelT265::align_yaw(const Vector3f &position, const Quaterni // trim yaw by difference between ahrs and sensor yaw const float yaw_trim_orig = _yaw_trim; _yaw_trim = wrap_2PI(yaw_rad - sens_yaw); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VisOdom: yaw shifted %d to %d deg", - (int)degrees(_yaw_trim - yaw_trim_orig), - (int)wrap_360(degrees(sens_yaw + _yaw_trim))); + + // warn user of significant change in yaw + if (fabsf(_yaw_trim - yaw_trim_orig) > radians(1.0f)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VisOdom: yaw shifted %d to %d deg", + (int)degrees(_yaw_trim - yaw_trim_orig), + (int)wrap_360(degrees(sens_yaw + _yaw_trim))); + } // convert _yaw_trim to _yaw_rotation to speed up processing later _yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim);