From 87960c04d8d5c170f9fefae81922a5f2d814e5ed Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 4 Mar 2024 12:57:58 -0500 Subject: [PATCH] mag_bias_estimator: sensor update loop limit iterations --- src/modules/mag_bias_estimator/MagBiasEstimator.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index 4ef331a071..e386170a21 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -157,10 +157,11 @@ void MagBiasEstimator::Run() bool updated = false; for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + int sensor_mag_updates = 0; sensor_mag_s sensor_mag; - while (_sensor_mag_subs[mag_index].update(&sensor_mag)) { - + while ((sensor_mag_updates < sensor_mag_s::ORB_QUEUE_LENGTH) && _sensor_mag_subs[mag_index].update(&sensor_mag)) { + sensor_mag_updates++; updated = true; // apply existing mag calibration