move IMU integration out of drivers to sensors hub to handle accel/gyro sync

- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu 
 - sensors: voted_sensors_update now consumes vehicle_imu
 - delete sensor_accel_integrated, sensor_gyro_integrated
 - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status
 - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -32,6 +32,8 @@
 ############################################################################
 
 px4_add_library(vehicle_imu
+	Integrator.cpp
+	Integrator.hpp
 	VehicleIMU.cpp
 	VehicleIMU.hpp
 )

src/modules/sensors/vehicle_imu/Integrator.cpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "Integrator.hpp"
+
+#include <drivers/drv_hrt.h>
+
+using matrix::Vector3f;
+
+bool Integrator::put(const hrt_abstime &timestamp, const Vector3f &val)
+{
+	if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) {
+		/* this is the first item in the integrator */
+		_last_integration_time = timestamp;
+		_last_reset_time = timestamp;
+		_last_val = val;
+
+		return false;
+	}
+
+	// Use trapezoidal integration to calculate the delta integral
+	const float dt = static_cast<float>(timestamp - _last_integration_time) * 1e-6f;
+	const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
+	_last_val = val;
+	_last_integration_time = timestamp;
+	_integrated_samples++;
+
+	// Calculate coning corrections if required
+	if (_coning_comp_on) {
+		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
+		// following:
+		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+		// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+		// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
+		_beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f;
+		_last_delta_alpha = delta_alpha;
+		_last_alpha = _alpha;
+	}
+
+	// accumulate delta integrals
+	_alpha += delta_alpha;
+
+	return true;
+}
+
+bool Integrator::reset(Vector3f &integral, uint32_t &integral_dt)
+{
+	if (integral_ready()) {
+		integral = Vector3f{_alpha};
+		_alpha.zero();
+
+		integral_dt = (_last_integration_time - _last_reset_time);
+		_last_reset_time = _last_integration_time;
+		_integrated_samples = 0;
+
+		// apply coning corrections if required
+		if (_coning_comp_on) {
+			integral += _beta;
+			_beta.zero();
+			_last_alpha.zero();
+		}
+
+		return true;
+	}
+
+	return false;
+}

src/modules/sensors/vehicle_imu/Integrator.hpp

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Integrator.hpp
+ *
+ * A resettable integrator
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#pragma once
+
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+class Integrator
+{
+public:
+	Integrator(bool coning_compensation = false) : _coning_comp_on(coning_compensation) {}
+	~Integrator() = default;
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param timestamp	Timestamp of the current value.
+	 * @param val		Item to put.
+	 * @return		true if data was accepted and integrated.
+	 */
+	bool put(const uint64_t &timestamp, const matrix::Vector3f &val);
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param timestamp	Timestamp of the current value.
+	 * @param val		Item to put.
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @return		true if putting the item triggered an integral reset and the integral should be
+	 *			published.
+	 */
+	bool put(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
+	{
+		return put(timestamp, val) && reset(integral, integral_dt);
+	}
+
+	/**
+	 * Set reset interval during runtime. This won't reset the integrator.
+	 *
+	 * @param reset_interval	    	New reset time interval for the integrator.
+	 */
+	void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; }
+
+	/**
+	 * Set required samples for reset. This won't reset the integrator.
+	 *
+	 * @param reset_samples	    	New reset time interval for the integrator.
+	 */
+	void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; }
+	uint8_t get_reset_samples() const { return _reset_samples_min; }
+
+	/**
+	 * Is the Integrator ready to reset?
+	 *
+	 * @return		true if integrator has sufficient data (minimum interval & samples satisfied) to reset.
+	 */
+	bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); }
+
+	/* Reset integrator and return current integral & integration time
+	 *
+	 * @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);
+
+private:
+	uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */
+	uint64_t _last_reset_time{0};       /**< last auto-announcement of integral value */
+
+	matrix::Vector3f _alpha{0.f, 0.f, 0.f};            /**< integrated value before coning corrections are applied */
+	matrix::Vector3f _last_alpha{0.f, 0.f, 0.f};       /**< previous value of _alpha */
+	matrix::Vector3f _beta{0.f, 0.f, 0.f};             /**< accumulated coning corrections */
+	matrix::Vector3f _last_val{0.f, 0.f, 0.f};         /**< previous input */
+	matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */
+
+	uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */
+
+	uint8_t _integrated_samples{0};
+	uint8_t _reset_samples_min{1};
+
+	const bool _coning_comp_on{false};                       /**< true to turn on coning corrections */
+};

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -35,20 +35,37 @@
 
 #include <px4_platform_common/log.h>
 
+#include <float.h>
+
 using namespace matrix;
 using namespace time_literals;
 
+using math::constrain;
+
 namespace sensors
 {
 
 VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
-	_sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index),
-	_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
+	_sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index),
+	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index),
 	_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
 	_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
 {
+	const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
+
+	_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);
+	_sensor_gyro_sub.set_required_updates(1);
+
+	// advertise immediately to ensure consistent ordering
+	_vehicle_imu_pub.advertise();
+	_vehicle_imu_status_pub.advertise();
 }
 
 VehicleIMU::~VehicleIMU()
@@ -61,14 +78,14 @@ bool VehicleIMU::Start()
 	// force initial updates
 	ParametersUpdate(true);
 
-	return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback();
+	return _sensor_gyro_sub.registerCallback();
 }
 
 void VehicleIMU::Stop()
 {
 	// clear all registered callbacks
-	_sensor_accel_integrated_sub.unregisterCallback();
-	_sensor_gyro_integrated_sub.unregisterCallback();
+	_sensor_accel_sub.unregisterCallback();
+	_sensor_gyro_sub.unregisterCallback();
 
 	Deinit();
 }
@@ -85,52 +102,251 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		_accel_corrections.ParametersUpdate();
 		_gyro_corrections.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);
+
+		if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
+			_param_imu_integ_rate.set(imu_integration_rate_hz);
+			_param_imu_integ_rate.commit_no_notification();
+		}
 	}
 }
 
+bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample)
+{
+	bool updated = false;
+
+	if ((intavg.timestamp_sample_last > 0) && (timestamp_sample > intavg.timestamp_sample_last)) {
+		intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last);
+		intavg.interval_count++;
+
+	} else {
+		intavg.interval_sum = 0.f;
+		intavg.interval_count = 0.f;
+	}
+
+	intavg.timestamp_sample_last = timestamp_sample;
+
+	// periodically calculate sensor update rate
+	if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
+
+		const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
+
+		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
+			// update if interval has changed by more than 1%
+			if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.01f) {
+
+				intavg.update_interval = sample_interval_avg;
+				updated = true;
+			}
+		}
+
+		// reset sample interval accumulator
+		intavg.timestamp_sample_last = 0;
+	}
+
+	return updated;
+}
+
 void VehicleIMU::Run()
 {
-	if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) {
-		sensor_accel_integrated_s accel;
-		_sensor_accel_integrated_sub.copy(&accel);
-		_accel_corrections.set_device_id(accel.device_id);
+	// backup schedule
+	ScheduleDelayed(10_ms);
 
-		sensor_gyro_integrated_s gyro;
-		_sensor_gyro_integrated_sub.copy(&gyro);
+	ParametersUpdate();
+	_accel_corrections.SensorCorrectionsUpdate();
+	_gyro_corrections.SensorCorrectionsUpdate();
+
+	bool update_integrator_config = false;
+
+	// integrate queued gyro
+	sensor_gyro_s gyro;
+
+	while (!_gyro_integrator.integral_ready() && _sensor_gyro_sub.update(&gyro)) {
 		_gyro_corrections.set_device_id(gyro.device_id);
+		_gyro_error_count = gyro.error_count;
 
-		ParametersUpdate();
-		_accel_corrections.SensorCorrectionsUpdate();
-		_gyro_corrections.SensorCorrectionsUpdate();
+		const Vector3f gyro_corrected{_gyro_corrections.Correct(Vector3f{gyro.x, gyro.y, gyro.z})};
+		_gyro_integrator.put(gyro.timestamp_sample, gyro_corrected);
+		_last_timestamp_sample_gyro = gyro.timestamp_sample;
 
-		// delta angle: apply offsets, scale, and board rotation
-		const float gyro_dt = 1.e-6f * gyro.dt;
-		Vector3f delta_angle = _gyro_corrections.Correct(Vector3f{gyro.delta_angle} * gyro_dt) / gyro_dt;
-
-		// delta velocity: apply offsets, scale, and board rotation
-		const float accel_dt = 1.e-6f * accel.dt;
-		Vector3f delta_velocity = _accel_corrections.Correct(Vector3f{accel.delta_velocity} * accel_dt) / accel_dt;
-
-		// publich vehicle_imu
-		vehicle_imu_s imu;
-		imu.timestamp_sample = accel.timestamp_sample;
-		imu.accel_device_id = accel.device_id;
-		imu.gyro_device_id = gyro.device_id;
-
-		delta_angle.copyTo(imu.delta_angle);
-		delta_velocity.copyTo(imu.delta_velocity);
-
-		imu.dt = accel.dt;
-		//imu.clip_count = accel.clip_count;
-		imu.timestamp = hrt_absolute_time();
-
-		_vehicle_imu_pub.publish(imu);
+		// collect sample interval average for filters
+		if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
+			update_integrator_config = true;
+		}
 	}
+
+	// update accel, stopping once caught up to the last gyro sample
+	sensor_accel_s accel;
+
+	while (_sensor_accel_sub.update(&accel)) {
+		_accel_corrections.set_device_id(accel.device_id);
+		_accel_error_count = accel.error_count;
+
+		const Vector3f accel_corrected{_accel_corrections.Correct(Vector3f{accel.x, accel.y, accel.z})};
+		_accel_integrator.put(accel.timestamp_sample, accel_corrected);
+		_last_timestamp_sample_accel = accel.timestamp_sample;
+
+		// collect sample interval average for filters
+		if (UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
+			update_integrator_config = true;
+		}
+
+		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
+
+			// rotate sensor clip counts into vehicle body frame
+			const Vector3f clipping{_accel_corrections.getBoardRotation() *
+				Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
+
+			// round to get reasonble clip counts per axis (after board rotation)
+			const uint8_t clip_x = roundf(fabsf(clipping(0)));
+			const uint8_t clip_y = roundf(fabsf(clipping(1)));
+			const uint8_t clip_z = roundf(fabsf(clipping(2)));
+
+			_delta_velocity_clipping_total[0] += clip_x;
+			_delta_velocity_clipping_total[1] += clip_y;
+			_delta_velocity_clipping_total[2] += clip_z;
+
+			if (clip_x > 0) {
+				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
+			}
+
+			if (clip_y > 0) {
+				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
+			}
+
+			if (clip_z > 0) {
+				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
+			}
+		}
+
+		// break once caught up to gyro
+		if (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval)) {
+			break;
+		}
+	}
+
+	// reconfigure integrators if calculated sensor intervals have changed
+	if (update_integrator_config) {
+		UpdateIntegratorConfiguration();
+	}
+
+	// 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;
+		Vector3f delta_angle;
+		Vector3f delta_velocity;
+
+		if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
+		    && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
+
+			UpdateAccelVibrationMetrics(delta_velocity);
+			UpdateGyroVibrationMetrics(delta_angle);
+
+			// vehicle_imu_status
+			//  publish first so that error counts are available synchronously if needed
+			vehicle_imu_status_s status;
+			status.accel_device_id = _accel_corrections.get_device_id();
+			status.gyro_device_id = _gyro_corrections.get_device_id();
+			status.accel_error_count = _accel_error_count;
+			status.gyro_error_count = _gyro_error_count;
+			status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
+			status.gyro_rate_hz = round(1e6f / _gyro_interval.update_interval);
+			status.accel_vibration_metric = _accel_vibration_metric;
+			status.gyro_vibration_metric = _gyro_vibration_metric;
+			status.gyro_coning_vibration = _gyro_coning_vibration;
+			status.accel_clipping[0] = _delta_velocity_clipping_total[0];
+			status.accel_clipping[1] = _delta_velocity_clipping_total[1];
+			status.accel_clipping[2] = _delta_velocity_clipping_total[2];
+			status.timestamp = hrt_absolute_time();
+			_vehicle_imu_status_pub.publish(status);
+
+
+			// publish vehicle_imu
+			vehicle_imu_s imu;
+			imu.timestamp_sample = _last_timestamp_sample_gyro;
+			imu.accel_device_id = _accel_corrections.get_device_id();
+			imu.gyro_device_id = _gyro_corrections.get_device_id();
+			delta_angle.copyTo(imu.delta_angle);
+			delta_velocity.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.timestamp = hrt_absolute_time();
+			_vehicle_imu_pub.publish(imu);
+
+			// reset clip counts
+			_delta_velocity_clipping = 0;
+
+			return;
+		}
+	}
+}
+
+void VehicleIMU::UpdateIntegratorConfiguration()
+{
+	if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) {
+
+		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);
+
+		// let the gyro set the configuration and scheduling
+		// accel integrator will be forced to reset when gyro integrator is ready
+		_gyro_integrator.set_reset_samples(gyro_integral_samples);
+		_accel_integrator.set_reset_samples(1);
+
+		// relaxed minimum integration time required
+		_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);
+
+		// run when there are enough new gyro samples, unregister accel
+		_sensor_accel_sub.unregisterCallback();
+
+		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d",
+			  _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples);
+	}
+}
+
+void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity)
+{
+	// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+	const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
+	_accel_vibration_metric = 0.99f * _accel_vibration_metric + 0.01f * delta_velocity_diff.norm();
+
+	_delta_velocity_prev = delta_velocity;
+}
+
+void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
+{
+	// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+	const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
+	_gyro_vibration_metric = 0.99f * _gyro_vibration_metric + 0.01f * delta_angle_diff.norm();
+
+	// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+	const Vector3f coning_metric = delta_angle % _delta_angle_prev;
+	_gyro_coning_vibration = 0.99f * _gyro_coning_vibration + 0.01f * coning_metric.norm();
+
+	_delta_angle_prev = delta_angle;
 }
 
 void VehicleIMU::PrintStatus()
 {
-	PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_corrections.get_device_id(), _gyro_corrections.get_device_id());
+	PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
+		 _accel_corrections.get_device_id(), (double)_accel_interval.update_interval,
+		 _gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval);
+
 	_accel_corrections.PrintStatus();
 	_gyro_corrections.PrintStatus();
 }

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include "Integrator.hpp"
+
 #include <sensor_corrections/SensorCorrections.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
@@ -40,19 +42,20 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_accel_integrated.h>
-#include <uORB/topics/sensor_gyro_integrated.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
 
 namespace sensors
 {
 
-class VehicleIMU : public ModuleParams, public px4::WorkItem
+class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	VehicleIMU() = delete;
@@ -69,13 +72,51 @@ private:
 	void ParametersUpdate(bool force = false);
 	void Run() override;
 
+	struct IntervalAverage {
+		hrt_abstime timestamp_sample_last{0};
+		float interval_sum{0.f};
+		float interval_count{0.f};
+		float update_interval{0.f};
+	};
+
+	bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample);
+	void UpdateIntegratorConfiguration();
+	void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
+	void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
+
 	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
+	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
-	uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub;
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub;
+	uORB::SubscriptionCallbackWorkItem _sensor_accel_sub;
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
 	SensorCorrections _accel_corrections;
 	SensorCorrections _gyro_corrections;
+
+	Integrator _accel_integrator{}; // 200 Hz default
+	Integrator _gyro_integrator{true};   // 200 Hz default, coning compensation enabled
+
+	hrt_abstime _last_timestamp_sample_accel{0};
+	hrt_abstime _last_timestamp_sample_gyro{0};
+
+	IntervalAverage _accel_interval{};
+	IntervalAverage _gyro_interval{};
+
+	uint32_t _accel_error_count{0};
+	uint32_t _gyro_error_count{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
+	float _accel_vibration_metric{0.f};	// high frequency vibration level in the IMU delta velocity data (m/s)
+	float _gyro_vibration_metric{0.f};	// high frequency vibration level in the IMU delta angle data (rad)
+	float _gyro_coning_vibration{0.f};	// Level of coning vibration in the IMU delta angles (rad^2)
+
+	uint8_t _delta_velocity_clipping{0};
+	uint32_t _delta_velocity_clipping_total[3] {};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
+	)
 };
 
 } // namespace sensors

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -35,6 +35,7 @@
 * IMU integration rate.
 *
 * The rate at which raw IMU data is integrated to produce delta angles and delta velocities.
+* Recommended to set this to a multiple of the estimator update period (currently 10 ms for ekf2).
 *
 * @min 100
 * @max 1000