sensors: VehicleIMU improve scheduling

- schedule on interval multiple of sensor_gyro publications without increasing the configured integration rate
 - IMU_INTEG_RATE add enums to guide appropriate selection
 - add average calculation bounds to prevent incorrect calculation and force monitoring again if there are consecutive gaps in data

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -108,19 +108,19 @@ void VehicleIMU::ParametersUpdate(bool force)
 		_accel_calibration.ParametersUpdate();
 		_gyro_calibration.ParametersUpdate();
 
-		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
-		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000);
+		// constrain IMU integration time 1-10 milliseconds (100-1000 Hz)
+		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
+						  100, math::max(_param_imu_gyro_ratemax.get(), 1000));
 
 		if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
+			PX4_WARN("IMU_INTEG_RATE updated %d -> %d", _param_imu_integ_rate.get(), imu_integration_rate_hz);
 			_param_imu_integ_rate.set(imu_integration_rate_hz);
 			_param_imu_integ_rate.commit_no_notification();
 		}
 
 		if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
 			// force update
-			_intervals_update = true;
-			_accel_interval.timestamp_sample_last = 0;
-			_gyro_interval.timestamp_sample_last = 0;
+			UpdateIntegratorConfiguration();
 		}
 	}
 }
@@ -129,8 +129,21 @@ bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstim
 {
 	bool updated = false;
 
-	if ((intavg.timestamp_sample_last > 0) && (timestamp_sample > intavg.timestamp_sample_last)) {
-		intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last);
+	// conservative maximum time between samples to reject large gaps and reset averaging
+	float max_interval_us = 10000; // 100 Hz
+	float min_interval_us = 100;   // 10,000 Hz
+
+	if (intavg.update_interval > 0) {
+		// if available use previously calculated interval as bounds
+		max_interval_us = 1.5f * intavg.update_interval;
+		min_interval_us = 0.5f * intavg.update_interval;
+	}
+
+	const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
+
+	if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
+
+		intavg.interval_sum += interval_us;
 		intavg.interval_count++;
 
 		// periodically calculate sensor update rate
@@ -170,6 +183,7 @@ void VehicleIMU::Run()
 
 	ParametersUpdate();
 
+	bool sensor_data_gap = false;
 	bool update_integrator_config = false;
 	bool publish_status = false;
 
@@ -180,11 +194,18 @@ void VehicleIMU::Run()
 		perf_count_interval(_gyro_update_perf, gyro.timestamp_sample);
 
 		if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
+			sensor_data_gap = true;
 			perf_count(_gyro_generation_gap_perf);
 
-			// if there's a gap in data start monitoring publication interval again
-			_intervals_update = true;
-			_gyro_interval.timestamp_sample_last = 0;
+			_gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging
+
+		} else {
+			// collect sample interval average for filters
+			if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
+				update_integrator_config = true;
+				publish_status = true;
+				_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
+			}
 		}
 
 		_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
@@ -199,14 +220,7 @@ void VehicleIMU::Run()
 		_gyro_integrator.put(gyro.timestamp_sample, Vector3f{gyro.x, gyro.y, gyro.z});
 		_last_timestamp_sample_gyro = gyro.timestamp_sample;
 
-		// collect sample interval average for filters
-		if (_intervals_update && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
-			update_integrator_config = true;
-			publish_status = true;
-			_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
-		}
-
-		if (_intervals_configured && _gyro_integrator.integral_ready()) {
+		if (!sensor_data_gap && _intervals_configured && _gyro_integrator.integral_ready()) {
 			break;
 		}
 	}
@@ -218,11 +232,18 @@ void VehicleIMU::Run()
 		perf_count_interval(_accel_update_perf, accel.timestamp_sample);
 
 		if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) {
+			sensor_data_gap = true;
 			perf_count(_accel_generation_gap_perf);
 
-			// if there's a gap in data start monitoring publication interval again
-			_intervals_update = true;
-			_accel_interval.timestamp_sample_last = 0;
+			_accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging
+
+		} else {
+			// collect sample interval average for filters
+			if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
+				update_integrator_config = true;
+				publish_status = true;
+				_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
+			}
 		}
 
 		_accel_last_generation = _sensor_accel_sub.get_last_generation();
@@ -237,13 +258,6 @@ void VehicleIMU::Run()
 		_accel_integrator.put(accel.timestamp_sample, Vector3f{accel.x, accel.y, accel.z});
 		_last_timestamp_sample_accel = accel.timestamp_sample;
 
-		// collect sample interval average for filters
-		if (_intervals_update && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
-			update_integrator_config = true;
-			publish_status = true;
-			_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
-		}
-
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
 
 			// rotate sensor clip counts into vehicle body frame
@@ -275,13 +289,25 @@ void VehicleIMU::Run()
 		}
 
 		// break once caught up to gyro
-		if (_intervals_configured
+		if (!sensor_data_gap && _intervals_configured
 		    && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) {
 
 			break;
 		}
 	}
 
+	if (sensor_data_gap) {
+		_consecutive_data_gap++;
+
+		// if there's consistently a gap in data start monitoring publication interval again
+		if (_consecutive_data_gap > 10) {
+			_intervals_configured = false;
+		}
+
+	} else {
+		_consecutive_data_gap = 0;
+	}
+
 	// reconfigure integrators if calculated sensor intervals have changed
 	if (update_integrator_config) {
 		UpdateIntegratorConfiguration();
@@ -349,11 +375,8 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
 
 		// determine number of sensor samples that will get closest to the desired integration interval
-		const uint8_t accel_integral_samples = constrain(roundf(configured_interval_us / _accel_interval.update_interval),
-						       1.f, (float)sensor_accel_s::ORB_QUEUE_LENGTH);
-
-		const uint8_t gyro_integral_samples = constrain(roundf(configured_interval_us / _gyro_interval.update_interval),
-						      1.f, (float)sensor_gyro_s::ORB_QUEUE_LENGTH);
+		const uint8_t accel_integral_samples = math::max(1.f, roundf(configured_interval_us / _accel_interval.update_interval));
+		const uint8_t gyro_integral_samples = math::max(1.f, roundf(configured_interval_us / _gyro_interval.update_interval));
 
 		// let the gyro set the configuration and scheduling
 		// accel integrator will be forced to reset when gyro integrator is ready
@@ -364,18 +387,23 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval.update_interval));
 		_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval.update_interval));
 
-		_sensor_accel_sub.set_required_updates(accel_integral_samples);
-		_sensor_gyro_sub.set_required_updates(gyro_integral_samples);
+		// gyro: find largest integer multiple of gyro_integral_samples
+		for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) {
+			if (gyro_integral_samples % n == 0) {
+				_sensor_gyro_sub.set_required_updates(n);
+
+				// run when there are enough new gyro samples, unregister accel
+				_sensor_accel_sub.unregisterCallback();
 
-		// run when there are enough new gyro samples, unregister accel
-		_sensor_accel_sub.unregisterCallback();
+				_intervals_configured = true; // stop monitoring topic publication rates
 
-		_intervals_configured = true;
-		_intervals_update = false; // stop monitoring topic publication rate
+				PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f sub samples: %d",
+					  _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
+					  (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval, n);
 
-		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
-			  _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
-			  (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
+				break;
+			}
+		}
 	}
 }
 
@@ -403,9 +431,14 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
 
 void VehicleIMU::PrintStatus()
 {
-	PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
-		 _accel_calibration.device_id(), (double)_accel_interval.update_interval,
-		 _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
+	if (_accel_calibration.device_id() == _gyro_calibration.device_id()) {
+		PX4_INFO("IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _accel_calibration.device_id(),
+			 (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
+
+	} else {
+		PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _accel_calibration.device_id(),
+			 (double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
+	}
 
 	perf_print_counter(_accel_generation_gap_perf);
 	perf_print_counter(_gyro_generation_gap_perf);

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -105,6 +105,7 @@ private:
 
 	unsigned _accel_last_generation{0};
 	unsigned _gyro_last_generation{0};
+	unsigned _consecutive_data_gap{0};
 
 	matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f};	// delta angle from the previous IMU measurement
 	matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f};	// delta velocity from the previous IMU measurement
@@ -113,7 +114,6 @@ private:
 
 	uint8_t _delta_velocity_clipping{0};
 
-	bool _intervals_update{true};
 	bool _intervals_configured{false};
 
 	perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};
@@ -122,7 +122,8 @@ private:
 	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
 	)
 };
 

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -39,6 +39,9 @@
 *
 * @min 100
 * @max 1000
+* @value 100 100 Hz
+* @value 200 200 Hz
+* @value 400 400 Hz
 * @unit Hz
 * @reboot_required true
 * @group Sensors