From 677f3e929457e125607524a66473ef9ef64f2012 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Oct 2022 13:45:04 -0400 Subject: [PATCH] sensors/vehicle_optical_flow: limit gyro updates and generation lost error per cycle - in the worst case scenario printing the error message can take longer than the next gyro publication, so combined with an infinite loop you could get stuck here --- .../vehicle_optical_flow/VehicleOpticalFlow.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index a6a7499a78..371ae74661 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -435,15 +435,21 @@ void VehicleOpticalFlow::UpdateSensorGyro() } // buffer - while (_sensor_gyro_sub.updated()) { - const unsigned last_generation = _sensor_gyro_sub.get_last_generation(); + bool sensor_gyro_lost_printed = false; + int gyro_updates = 0; + while (_sensor_gyro_sub.updated() && (gyro_updates < sensor_gyro_s::ORB_QUEUE_LENGTH)) { + gyro_updates++; + const unsigned last_generation = _sensor_gyro_sub.get_last_generation(); sensor_gyro_s sensor_gyro; if (_sensor_gyro_sub.copy(&sensor_gyro)) { if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation()); + if (!sensor_gyro_lost_printed) { + PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation()); + sensor_gyro_lost_printed = true; + } } _gyro_calibration.set_device_id(sensor_gyro.device_id);