AP_VisualOdom: reduce yaw alignment spamming

rmackay9 · Nov 20, 2024 · e78336d · e78336d
1 parent 4693ab7
commit e78336d
Showing 1 changed file with 7 additions and 3 deletions.
diff --git 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);