From 8b6c70e0f2992bc5add859cd427489f33d3bbf62 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 4 Mar 2024 13:07:32 -0500
Subject: [PATCH] sensors/vehicle_angular_velocity: sensor update loop limit
 iterations

---
 .../VehicleAngularVelocity.cpp                         | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
index f2b0d3540b..b1eac45152 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -822,9 +822,12 @@ void VehicleAngularVelocity::Run()
 
 	if (_fifo_available) {
 		// process all outstanding fifo messages
+		int sensor_sub_updates = 0;
 		sensor_gyro_fifo_s sensor_fifo_data;
 
-		while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
+		while ((sensor_sub_updates < sensor_gyro_fifo_s::ORB_QUEUE_LENGTH) && _sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
+			sensor_sub_updates++;
+
 			const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
 			const int N = sensor_fifo_data.samples;
 			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
@@ -863,9 +866,12 @@ void VehicleAngularVelocity::Run()
 
 	} else {
 		// process all outstanding messages
+		int sensor_sub_updates = 0;
 		sensor_gyro_s sensor_data;
 
-		while (_sensor_sub.update(&sensor_data)) {
+		while ((sensor_sub_updates < sensor_gyro_s::ORB_QUEUE_LENGTH) && _sensor_sub.update(&sensor_data)) {
+			sensor_sub_updates++;
+
 			if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) {
 
 				if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {