sensors/vehicle_imu: continue integrating queued raw data if falling behind

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -133,6 +133,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 			_param_imu_integ_rate.commit_no_notification();
 		}
 
+		_imu_integration_interval_us = 1000000 / imu_integration_rate_hz;
+
 		if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
 			// force update
 			UpdateIntegratorConfiguration();
@@ -245,7 +247,10 @@ void VehicleIMU::Run()
 		_gyro_integrator.put(gyro.timestamp_sample, gyro_raw);
 		_last_timestamp_sample_gyro = gyro.timestamp_sample;
 
-		if (!sensor_data_gap && _intervals_configured && _gyro_integrator.integral_ready()) {
+		// break if interval is configured and we haven't fallen behind
+		if (_intervals_configured && _gyro_integrator.integral_ready()
+		    && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) {
+
 			break;
 		}
 	}

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -100,6 +100,8 @@ private:
 	hrt_abstime _last_timestamp_sample_accel{0};
 	hrt_abstime _last_timestamp_sample_gyro{0};
 
+	uint32_t _imu_integration_interval_us{4000};
+
 	IntervalAverage _accel_interval{};
 	IntervalAverage _gyro_interval{};