From d5eea0dd924b8581e3d4216d41106c36c71b48c1 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Mon, 8 Sep 2025 11:49:27 +0300 Subject: [PATCH] sensors/VehicleImu: Don't set _backup_schedule_timeout_us shorter than normal scheduling interval With IMUs of higher report rate (e.g. ADIS16470), setting backup schedule timeout simply to half of the ORB queue length may cause the backup firing before required updates are received. Set backup schedule to queue length - 1 instead. Additionally, double-check that the backup doesn't get too short after finding the largest integer multiple of gyro_integral_samples. Signed-off-by: Jukka Laitinen --- src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index b8c0a9547e..7ba258a66b 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -706,8 +706,8 @@ void VehicleIMU::UpdateIntegratorConfiguration() _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * gyro_interval_us)); _gyro_integrator.set_reset_samples(gyro_integral_samples); - _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * gyro_interval_us) / 2, 1000, 20000); + _backup_schedule_timeout_us = math::constrain((int)math::min((sensor_accel_s::ORB_QUEUE_LENGTH - 1) * accel_interval_us, + (sensor_gyro_s::ORB_QUEUE_LENGTH - 1) * gyro_interval_us), 1000, 20000); // gyro: find largest integer multiple of gyro_integral_samples for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { @@ -716,6 +716,12 @@ void VehicleIMU::UpdateIntegratorConfiguration() } if (gyro_integral_samples % n == 0) { + // Make sure _backup_schedule_timeout_us is not smaller than normal scheduling interval + + if (_backup_schedule_timeout_us < n * gyro_interval_us) { + _backup_schedule_timeout_us = (n + 1) * gyro_interval_us; + } + _sensor_gyro_sub.set_required_updates(n); _sensor_gyro_sub.registerCallback();