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

---
 src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
index 2e50a94608..f5b7343496 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
@@ -194,7 +194,12 @@ void VehicleIMU::Run()
 	// reset data gap monitor
 	_data_gap = false;
 
-	while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) {
+	int sensor_sub_updates = 0;
+
+	while ((_sensor_gyro_sub.updated() || _sensor_accel_sub.updated())
+	       && (sensor_sub_updates < math::max(sensor_accel_s::ORB_QUEUE_LENGTH, sensor_gyro_s::ORB_QUEUE_LENGTH))) {
+		sensor_sub_updates++;
+
 		bool updated = false;
 
 		bool consume_all_gyro = !_intervals_configured || _data_gap;
@@ -222,11 +227,16 @@ void VehicleIMU::Run()
 
 
 		// update accel until integrator ready and caught up to gyro
+		int sensor_accel_sub_updates = 0;
+
 		while (_sensor_accel_sub.updated()
+		       && (sensor_accel_sub_updates < sensor_accel_s::ORB_QUEUE_LENGTH)
 		       && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap
 			   || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))
 		      ) {
 
+			sensor_accel_sub_updates++;
+
 			if (UpdateAccel()) {
 				updated = true;
 			}