fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 17:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/VehicleImu: Don't set _backup_schedule_timeout_us shorter than normal scheduling interval

Browse Source 
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 <jukka.laitinen@tii.ae>
This commit is contained in:
Jukka Laitinen
2025-09-08 11:49:27 +03:00
committed by Jacob Dahl
parent 0c9ebc4321
commit d5eea0dd92
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+8 -2
View File
@@ -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();