From 2adc23fd97a07ca24cbc59c376d9e1e0d9cd664c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jul 2020 10:05:11 -0400 Subject: [PATCH] sensors/vehicle_imu: initially schedule conservatively - this lowers the load immediately at boot while still allowing each VehicleIMU instance to properly measure the actual accel/gyro data rates - for lockstep builds this is ignored and VehicleIMU initially runs with every new sensor_accel/sensor_gyro publication --- src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 6db9ebc3ef..3ad32242d2 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -55,11 +55,19 @@ VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) : _accel_integrator.set_reset_interval(configured_interval_us); _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); - _sensor_accel_sub.set_required_updates(1); _gyro_integrator.set_reset_interval(configured_interval_us); _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // currently with lockstep every raw sample needs a corresponding vehicle_imu publication + _sensor_accel_sub.set_required_updates(1); _sensor_gyro_sub.set_required_updates(1); +#else + // schedule conservatively until the actual accel & gyro rates are known + _sensor_accel_sub.set_required_updates(sensor_accel_s::ORB_QUEUE_LENGTH / 2); + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); +#endif // advertise immediately to ensure consistent ordering _vehicle_imu_pub.advertise();