sensors/vehicle_imu: minor IMU integration improvements

- IMU integrator set max dt based on final return size (uint16)
 - improve integration consuming gyro as needed then integrate all available accel until caught up
 - increase required number of samples for sensor rate measurement (online Welford mean)

src/modules/sensors/vehicle_imu/Integrator.hpp

@@ -51,6 +51,9 @@ public:
 	Integrator() = default;
 	~Integrator() = default;
 
+	static constexpr float DT_MIN{FLT_MIN};
+	static constexpr float DT_MAX{static_cast<float>(UINT16_MAX) * 1e-6f};
+
 	/**
 	 * Put an item into the integral.
 	 *
@@ -60,7 +63,7 @@ public:
 	 */
 	inline void put(const matrix::Vector3f &val, const float dt)
 	{
-		if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
+		if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) {
 			_alpha += integrate(val, dt);
 
 		} else {
@@ -103,7 +106,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 * @return		true if integral valid
 	 */
-	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
+	bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
 	{
 		if (integral_ready()) {
 			integral = _alpha;
@@ -155,7 +158,7 @@ public:
 	 */
 	inline void put(const matrix::Vector3f &val, const float dt)
 	{
-		if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
+		if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) {
 			// Use trapezoidal integration to calculate the delta integral
 			const matrix::Vector3f delta_alpha{integrate(val, dt)};
 
@@ -190,7 +193,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 * @return		true if integral valid
 	 */
-	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
+	bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
 	{
 		if (Integrator::reset(integral, integral_dt)) {
 			// apply coning corrections

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -158,10 +158,10 @@ void VehicleIMU::Run()
 		bool consume_all_gyro = !_intervals_configured || _data_gap;
 
 		// monitor scheduling latency and force catch up with latest gyro if falling behind
-		if (_sensor_gyro_sub.updated() && _gyro_update_latency_mean.valid()
-		    && (_gyro_update_latency_mean.mean()(1) > (1.5f * _gyro_interval_us * 1e-6f))) {
+		if (_sensor_gyro_sub.updated() && (_gyro_update_latency_mean.count() > 100)
+		    && (_gyro_update_latency_mean.mean()(1) > _gyro_interval_us * 1e-6f)) {
 
-			PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f us",
+			PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f",
 				  (double)_gyro_update_latency_mean.mean()(0),
 				  (double)_gyro_update_latency_mean.mean()(1));
 
@@ -176,11 +176,11 @@ void VehicleIMU::Run()
 		}
 
 
-		bool consume_all_accel = !_intervals_configured || _data_gap
-					 || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us));
-
 		// update accel until integrator ready and caught up to gyro
-		if (!_accel_integrator.integral_ready() || consume_all_accel) {
+		while (_sensor_accel_sub.updated()
+		       && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap
+			   || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))) {
+
 			if (UpdateAccel()) {
 				updated = true;
 			}
@@ -192,11 +192,12 @@ void VehicleIMU::Run()
 		}
 
 		// check for additional updates and that we're fully caught up before publishing
-		if ((consume_all_gyro && _sensor_gyro_sub.updated()) || (consume_all_accel && _sensor_accel_sub.updated())) {
+		if (consume_all_gyro && _sensor_gyro_sub.updated()) {
 			continue;
 		}
 
-		if (_intervals_configured) {
+		// publish if both accel & gyro integrators are ready
+		if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
 			if (Publish()) {
 				// record gyro publication latency and integrated samples
 				if (_gyro_update_latency_mean.count() > 10000) {
@@ -205,9 +206,8 @@ void VehicleIMU::Run()
 				}
 
 				const float time_run_s = now_us * 1e-6f;
-				const float time_gyro_timestamp_last_s = _gyro_timestamp_last * 1e-6f;
-				const float time_gyro_timestamp_sample_last_s = _gyro_timestamp_sample_last * 1e-6f;
-				_gyro_update_latency_mean.update(Vector2f{time_run_s - time_gyro_timestamp_sample_last_s, time_run_s - time_gyro_timestamp_last_s});
+
+				_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
 
 				return;
 			}
@@ -242,7 +242,7 @@ bool VehicleIMU::UpdateAccel()
 				_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
 			}
 
-			if (_accel_interval_mean.valid()
+			if (_accel_interval_mean.valid() && (_accel_interval_mean.count() > 100 || !PX4_ISFINITE(_accel_interval_best_variance))
 			    && ((_accel_interval_mean.variance()(0) < _accel_interval_best_variance) || (_accel_interval_mean.count() > 1000))) {
 				// update sample rate if previously invalid or changed
 				const float interval_delta_us = fabsf(_accel_interval_mean.mean()(0) - _accel_interval_us);
@@ -340,7 +340,7 @@ bool VehicleIMU::UpdateGyro()
 	// integrate queued gyro
 	sensor_gyro_s gyro;
 
-	while (_sensor_gyro_sub.update(&gyro)) {
+	if (_sensor_gyro_sub.update(&gyro)) {
 		if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
 			_data_gap = true;
 			perf_count(_gyro_generation_gap_perf);
@@ -355,7 +355,7 @@ bool VehicleIMU::UpdateGyro()
 				_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
 			}
 
-			if (_gyro_interval_mean.valid()
+			if (_gyro_interval_mean.valid() && (_gyro_interval_mean.count() > 100 || !PX4_ISFINITE(_gyro_interval_best_variance))
 			    && ((_gyro_interval_mean.variance()(0) < _gyro_interval_best_variance) || (_gyro_interval_mean.count() > 1000))) {
 				// update sample rate if previously invalid or changed
 				const float interval_delta_us = fabsf(_gyro_interval_mean.mean()(0) - _gyro_interval_us);
@@ -411,27 +411,23 @@ bool VehicleIMU::Publish()
 {
 	bool updated = false;
 
-	// publish if both accel & gyro integrators are ready
-	if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
-
-		uint32_t accel_integral_dt;
-		uint32_t gyro_integral_dt;
+	vehicle_imu_s imu;
 	Vector3f delta_angle;
 	Vector3f delta_velocity;
 
-		if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
-		    && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
+	if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt)
+	    && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) {
 
 		if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
 
 			// delta angle: apply offsets, scale, and board rotation
 			_gyro_calibration.SensorCorrectionsUpdate();
-				const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
+			const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt;
 			const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
 
 			// delta velocity: apply offsets, scale, and board rotation
 			_accel_calibration.SensorCorrectionsUpdate();
-				const float accel_dt_inv = 1.e6f / accel_integral_dt;
+			const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
 			Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
 
 			UpdateAccelVibrationMetrics(delta_velocity_corrected);
@@ -439,7 +435,9 @@ bool VehicleIMU::Publish()
 
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
-				if (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
+			if ((_accel_sum_count > 0) && (_gyro_sum_count > 0)
+			    && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
+
 				_status.accel_device_id = _accel_calibration.device_id();
 				_status.gyro_device_id = _gyro_calibration.device_id();
 
@@ -465,26 +463,21 @@ bool VehicleIMU::Publish()
 				_publish_status = false;
 			}
 
-
 			// publish vehicle_imu
-				vehicle_imu_s imu;
 			imu.timestamp_sample = _gyro_timestamp_sample_last;
 			imu.accel_device_id = _accel_calibration.device_id();
 			imu.gyro_device_id = _gyro_calibration.device_id();
 			delta_angle_corrected.copyTo(imu.delta_angle);
 			delta_velocity_corrected.copyTo(imu.delta_velocity);
-				imu.delta_angle_dt = gyro_integral_dt;
-				imu.delta_velocity_dt = accel_integral_dt;
 			imu.delta_velocity_clipping = _delta_velocity_clipping;
 			imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
 			imu.timestamp = hrt_absolute_time();
 			_vehicle_imu_pub.publish(imu);
 
-				updated = true;
-			}
-
 			// reset clip counts
 			_delta_velocity_clipping = 0;
+
+			updated = true;
 		}
 	}
 
@@ -513,11 +506,12 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us));
 		_accel_integrator.set_reset_samples(accel_integral_samples);
 
-		_backup_schedule_timeout_us = sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us;
-
 		_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::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
+							sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us);
+
 		// 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 > sensor_gyro_s::ORB_QUEUE_LENGTH) {
@@ -565,12 +559,15 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
 
 void VehicleIMU::PrintStatus()
 {
-
 	PX4_INFO("%" PRIu8 " - Accel ID: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro ID: %" PRIu32
 		 ", interval: %.1f us (SD %.1f us)",
 		 _instance, _accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance),
 		 _gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance));
 
+	PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s",
+		  (double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1),
+		  (double)(_gyro_interval_us * 1e-6f));
+
 	perf_print_counter(_accel_generation_gap_perf);
 	perf_print_counter(_gyro_generation_gap_perf);