sensors/vehicle_angular_velocity: use full raw FIFO data (sensor_gyro_fifo) if available

boards/nxp/fmurt1062-v1/default.cmake

@@ -42,7 +42,7 @@ px4_add_board(
 		pwm_out_sim
 		pwm_out
 		rc_input
-		roboclaw
+		#roboclaw
 		safety_button
 		telemetry # all available telemetry drivers
 		tone_alarm

msg/sensor_gyro_fifo.msg

@@ -13,3 +13,5 @@ int16[32] y               # angular velocity in the FRD board frame Y-axis in ra
 int16[32] z               # angular velocity in the FRD board frame Z-axis in rad/s
 
 uint8 rotation            # Direction the sensor faces (see Rotation enum)
+
+uint8 ORB_QUEUE_LENGTH = 2

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -48,7 +48,7 @@ struct wq_config_t {
 
 namespace wq_configurations
 {
-static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority
+static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1888, 0}; // PX4 inner loop highest priority
 static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl
 
 static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1};

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -33,8 +33,6 @@
 
 #include "LowPassFilter2p.hpp"
 
-#include <px4_platform_common/defines.h>
-
 #include <math.h>
 
 namespace math
@@ -72,25 +70,6 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 	_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
 }
 
-float LowPassFilter2p::apply(float sample)
-{
-	// do the filtering
-	float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
-
-	if (!PX4_ISFINITE(delay_element_0)) {
-		// don't allow bad values to propagate via the filter
-		delay_element_0 = sample;
-	}
-
-	const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
-
-	_delay_element_2 = _delay_element_1;
-	_delay_element_1 = delay_element_0;
-
-	// return the value. Should be no need to check limits
-	return output;
-}
-
 float LowPassFilter2p::reset(float sample)
 {
 	const float dval = sample / (_b0 + _b1 + _b2);

src/lib/mathlib/math/filter/LowPassFilter2p.hpp

@@ -38,6 +38,8 @@
 
 #pragma once
 
+#include <px4_platform_common/defines.h>
+
 namespace math
 {
 class __EXPORT LowPassFilter2p
@@ -58,7 +60,23 @@ public:
 	 *
 	 * @return retrieve the filtered result
 	 */
-	float apply(float sample);
+	inline float apply(float sample)
+	{
+		// Direct Form II implementation
+		float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+
+		if (!PX4_ISFINITE(delay_element_0)) {
+			// don't allow bad values to propagate via the filter
+			delay_element_0 = sample;
+		}
+
+		const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+
+		_delay_element_2 = _delay_element_1;
+		_delay_element_1 = delay_element_0;
+
+		return output;
+	}
 
 	// Return the cutoff frequency
 	float get_cutoff_freq() const { return _cutoff_freq; }

src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp

@@ -51,35 +51,23 @@ public:
 	{
 	}
 
-	/**
+	// Filter array of samples in place using the Direct form II.
-	 * Add a new raw value to the filter
+	inline void apply(float samples[], uint8_t num_samples)
-	 *
-	 * @return retrieve the filtered result
-	 */
-	inline float apply(const int16_t samples[], uint8_t num_samples)
 	{
-		float output = 0.0f;
-
 		for (int n = 0; n < num_samples; n++) {
-			// do the filtering
+			// Direct Form II implementation
-			float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+			float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
 
-			if (n == num_samples - 1) {
+			// don't allow bad values to propagate via the filter
-				output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+			if (!PX4_ISFINITE(delay_element_0)) {
+				delay_element_0 = samples[n];
 			}
 
+			samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+
 			_delay_element_2 = _delay_element_1;
 			_delay_element_1 = delay_element_0;
 		}
-
-		// don't allow bad values to propagate via the filter
-		if (!PX4_ISFINITE(output)) {
-			reset(samples[num_samples - 1]);
-			output = samples[num_samples - 1];
-		}
-
-		// return the value. Should be no need to check limits
-		return output;
 	}
 
 };

src/lib/mathlib/math/filter/NotchFilterArray.hpp

@@ -64,11 +64,7 @@ public:
 	NotchFilterArray() = default;
 	~NotchFilterArray() = default;
 
-	/**
+	// Filter array of samples in place using the Direct form II.
-	 * Add new raw values to the filter using the Direct form II.
-	 *
-	 * @return retrieve the filtered result
-	 */
 	inline void apply(T samples[], uint8_t num_samples)
 	{
 		for (int n = 0; n < num_samples; n++) {
@@ -96,7 +92,7 @@ public:
 	{
 		for (int n = 0; n < num_samples; n++) {
 			// Direct Form II implementation
-			const T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 -
+			T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 -
 				   _a2 * _delay_element_output_2;
 
 			// don't allow bad values to propagate via the filter

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2021 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
@@ -46,7 +46,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	CheckAndUpdateFilters();
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()
@@ -81,46 +80,80 @@ void VehicleAngularVelocity::Stop()
 	Deinit();
 }
 
-void VehicleAngularVelocity::CheckAndUpdateFilters()
+bool VehicleAngularVelocity::UpdateSampleRate()
 {
-	bool sample_rate_changed = false;
+	float sample_rate_hz = NAN;
+	float publish_rate_hz = NAN;
 
-	// get sample rate from vehicle_imu_status publication
 	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
 
-		const float sample_rate_hz = imu_status.get().gyro_rate_hz;
+		if (imu_status.get().gyro_device_id == _selected_sensor_device_id) {
-
+			sample_rate_hz = imu_status.get().gyro_raw_rate_hz;
-		if ((imu_status.get().gyro_device_id != 0) && (imu_status.get().gyro_device_id == _calibration.device_id())
+			publish_rate_hz = imu_status.get().gyro_rate_hz;
-		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
-			// check if sample rate error is greater than 1%
-			if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
-				_filter_sample_rate = sample_rate_hz;
-				sample_rate_changed = true;
 			break;
 		}
 	}
+
+	// calculate sensor update rate
+	if (PX4_ISFINITE(sample_rate_hz) && PX4_ISFINITE(publish_rate_hz)) {
+		// check if sample rate error is greater than 1%
+		if ((fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) {
+			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz);
+			_reset_filters = true;
+			_filter_sample_rate_hz = sample_rate_hz;
+
+			if (_param_imu_gyro_ratemax.get() > 0.f) {
+				// determine number of sensor samples that will get closest to the desired rate
+				const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get();
+				const float publish_interval_us = 1e6f / publish_rate_hz;
+
+				const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
+
+				if (_fifo_available) {
+					_sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH));
+
+				} else {
+					_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH));
 				}
 
-	// update software low pass filters
+				// publish interval (constrained 100 Hz - 8 kHz)
-	if (sample_rate_changed || (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.1f)) {
+				_publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us / 2.f)), 125,
-		_lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
+							   10000);
-		_lp_filter_velocity.reset(_angular_velocity_prev);
+
+			} else {
+				_sensor_sub.set_required_updates(1);
+				_sensor_fifo_sub.set_required_updates(1);
+				_publish_interval_min_us = 0;
+			}
 		}
 
-	if (sample_rate_changed
+		if (_filter_sample_rate_hz > 0.f) {
-	    || (fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.1f)
+			return true;
-	    || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.1f)
+		}
-	   ) {
-		_notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
-		_notch_filter_velocity.reset(_angular_velocity_prev);
 	}
 
-	if (sample_rate_changed || (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.1f)) {
+	return false;
-		_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
-		_lp_filter_acceleration.reset(_angular_acceleration_prev);
 }
+
+void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration)
+{
+	for (int axis = 0; axis < 3; axis++) {
+		// angular velocity low pass
+		_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
+		_lp_filter_velocity[axis].reset(angular_velocity(axis));
+
+		// angular velocity notch
+		_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
+				_param_imu_gyro_nf_bw.get());
+		_notch_filter_velocity[axis].reset(angular_velocity(axis));
+
+		// angular acceleration low pass
+		_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
+		_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
+	}
+
+	_reset_filters = false;
 }
 
 void VehicleAngularVelocity::SensorBiasUpdate(bool force)
@@ -137,8 +170,7 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 
-		if (_estimator_sensor_bias_sub.copy(&bias)) {
+		if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) {
-			if (bias.gyro_device_id == _calibration.device_id()) {
 			_bias = Vector3f{bias.gyro_bias};
 
 		} else {
@@ -146,31 +178,55 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 		}
 	}
 }
-}
 
 bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 {
-	if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) {
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
 		sensor_selection_s sensor_selection{};
 		_sensor_selection_sub.copy(&sensor_selection);
 
-		if ((sensor_selection.gyro_device_id != 0) && (_calibration.device_id() != sensor_selection.gyro_device_id)) {
+		if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
+
+			// see if the selected sensor publishes sensor_gyro_fifo
+			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+				uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
+
+				if ((sensor_gyro_fifo_sub.get().device_id != 0)
+				    && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) {
+					if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
+						// make sure non-FIFO sub is unregistered
+						_sensor_sub.unregisterCallback();
+
+						// record selected sensor
+						_selected_sensor_device_id = sensor_selection.gyro_device_id;
+						_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
+
+						_reset_filters = true;
+						_bias.zero();
+						_fifo_available = true;
+
+						return true;
+					}
+				}
+			}
+
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
 
-				const uint32_t device_id = sensor_gyro_sub.get().device_id;
+				if ((sensor_gyro_sub.get().device_id != 0)
-
+				    && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) {
-				if ((device_id != 0) && (device_id == sensor_selection.gyro_device_id)) {
-
 					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-						PX4_DEBUG("selected sensor changed %d -> %d", _calibration.device_id(), device_id);
+						// make sure FIFO sub is unregistered
+						_sensor_fifo_sub.unregisterCallback();
+
+						// record selected sensor
+						_calibration.set_device_id(sensor_gyro_sub.get().device_id);
+						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 
 						// clear bias and corrections
+						_reset_filters = true;
 						_bias.zero();
-
+						_fifo_available = false;
-						_calibration.set_device_id(device_id);
-
-						CheckAndUpdateFilters();
 
 						return true;
 					}
@@ -178,7 +234,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 			}
 
 			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
-			_calibration.set_device_id(0);
+			_selected_sensor_device_id = 0;
 		}
 	}
 
@@ -197,7 +253,31 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		_calibration.ParametersUpdate();
 
-		CheckAndUpdateFilters();
+		// gyro low pass cutoff frequency changed
+		for (auto &lp : _lp_filter_velocity) {
+			if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {
+				_reset_filters = true;
+				break;
+			}
+		}
+
+		// gyro notch filter frequency or bandwidth changed
+		for (auto &nf : _notch_filter_velocity) {
+			if ((fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
+			    || (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)) {
+
+				_reset_filters = true;
+				break;
+			}
+		}
+
+		// gyro derivative low pass cutoff changed
+		for (auto &lp : _lp_filter_acceleration) {
+			if (fabsf(lp.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+				_reset_filters = true;
+				break;
+			}
+		}
 	}
 }
 
@@ -207,81 +287,162 @@ void VehicleAngularVelocity::Run()
 	ScheduleDelayed(10_ms);
 
 	// update corrections first to set _selected_sensor
-	bool selection_updated = SensorSelectionUpdate();
+	const bool selection_updated = SensorSelectionUpdate();
 
 	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
+	if (_fifo_available) {
+		// process all outstanding fifo messages
+		sensor_gyro_fifo_s sensor_fifo_data;
+
+		while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
+			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
+
+			if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) {
+				const int N = sensor_fifo_data.samples;
+				const float dt_s = sensor_fifo_data.dt * 1e-6f;
+				const enum Rotation fifo_rotation = static_cast<enum Rotation>(sensor_fifo_data.rotation);
+
+				if (_reset_filters || (fabsf(sensor_fifo_data.scale - _fifo_last_scale) > FLT_EPSILON)) {
+					if (UpdateSampleRate()) {
+						// in FIFO mode the unscaled raw data is filtered
+						_angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale;
+						ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale);
+
+						_fifo_last_scale = sensor_fifo_data.scale;
+					}
+
+					if (_reset_filters) {
+						continue; // not safe to run until filters configured
+					}
+				}
+
+				Vector3f angular_velocity_unscaled;
+				Vector3f angular_acceleration_unscaled;
+
+				int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z};
+
+				for (int axis = 0; axis < 3; axis++) {
+					// copy raw int16 sensor samples to float array for filtering
+					float data[FIFO_SIZE_MAX];
+
+					for (int n = 0; n < N; n++) {
+						data[n] = raw_data_array[axis][n];
+					}
+
+					// Apply general notch filter (IMU_GYRO_NF_FREQ)
+					if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
+						_notch_filter_velocity[axis].apply(data, N);
+					}
+
+					// Apply general low-pass filter (IMU_GYRO_CUTOFF)
+					_lp_filter_velocity[axis].apply(data, N);
+
+					// save last filtered sample
+					angular_velocity_unscaled(axis) = data[N - 1];
+
+
+					// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+					float delta_velocity_filtered;
+
+					for (int n = 0; n < N; n++) {
+						const float delta_velocity = (data[n] - _angular_velocity_prev(axis));
+						delta_velocity_filtered = _lp_filter_acceleration[axis].apply(delta_velocity);
+						_angular_velocity_prev(axis) = data[n];
+					}
+
+					angular_acceleration_unscaled(axis) = delta_velocity_filtered / dt_s;
+				}
+
+				// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
+				rotate_3f(fifo_rotation, angular_velocity_unscaled(0), angular_velocity_unscaled(1), angular_velocity_unscaled(2));
+				_angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias;
+
+				// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
+				rotate_3f(fifo_rotation, angular_acceleration_unscaled(0), angular_acceleration_unscaled(1),
+					  angular_acceleration_unscaled(2));
+				_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale;
+
+				// Publish
+				if (!_sensor_fifo_sub.updated() && (sensor_fifo_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
+					Publish(sensor_fifo_data.timestamp_sample);
+				}
+			}
+		}
+
+	} else {
 		// process all outstanding messages
 		sensor_gyro_s sensor_data;
 
 		while (_sensor_sub.update(&sensor_data)) {
+			const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f);
+			_timestamp_sample_last = sensor_data.timestamp_sample;
 
-		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+			if (_reset_filters) {
-		const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f);
+				if (UpdateSampleRate()) {
-		_timestamp_sample_prev = sensor_data.timestamp_sample;
+					_angular_velocity_prev = _angular_velocity;
+					ResetFilters(_angular_velocity, _angular_acceleration);
+				}
 
-		// get the sensor data and correct for thermal errors (apply offsets and scale)
+				if (_reset_filters) {
-		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
+					continue; // not safe to run until filters configured
-
-		// correct for in-run bias errors
-		const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
-
-		// Gyro filtering:
-		// - Apply general notch filter (IMU_GYRO_NF_FREQ)
-		// - Apply general low-pass filter (IMU_GYRO_CUTOFF)
-		// - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
-
-		const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
-
-		const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
-
-		const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
-		_angular_velocity_prev = angular_velocity;
-		_angular_acceleration_prev = angular_acceleration_raw;
-		const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
-
-
-		// publish once all new samples are processed
-		if (!_sensor_sub.updated()) {
-			bool publish = true;
-
-			if (_param_imu_gyro_rate_max.get() > 0) {
-				const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
-
-				if (hrt_elapsed_time(&_last_publish) < interval) {
-					publish = false;
 				}
 			}
 
-			if (publish) {
+			// Apply calibration, rotation, and correct for in-run bias errors
+			Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias};
+
+			for (int axis = 0; axis < 3; axis++) {
+				// Apply general notch filter (IMU_GYRO_NF_FREQ)
+				_notch_filter_velocity[axis].apply(&angular_velocity(axis), 1);
+
+				// Apply general low-pass filter (IMU_GYRO_CUTOFF)
+				_lp_filter_velocity[axis].apply(&angular_velocity(axis), 1);
+
+				// Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+				const float accel = (angular_velocity(axis) - _angular_velocity_prev(axis)) / dt_s;
+				_angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel);
+				_angular_velocity_prev(axis) = angular_velocity(axis);
+			}
+
+			_angular_velocity = angular_velocity;
+
+			// Publish
+			if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
+				Publish(sensor_data.timestamp_sample);
+			}
+		}
+	}
+}
+
+void VehicleAngularVelocity::Publish(const hrt_abstime &timestamp_sample)
+{
 	// Publish vehicle_angular_acceleration
 	vehicle_angular_acceleration_s v_angular_acceleration;
-				v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
+	v_angular_acceleration.timestamp_sample = timestamp_sample;
-				angular_acceleration.copyTo(v_angular_acceleration.xyz);
+	_angular_acceleration.copyTo(v_angular_acceleration.xyz);
 	v_angular_acceleration.timestamp = hrt_absolute_time();
 	_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
 
 	// Publish vehicle_angular_velocity
 	vehicle_angular_velocity_s v_angular_velocity;
-				v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
+	v_angular_velocity.timestamp_sample = timestamp_sample;
-				angular_velocity.copyTo(v_angular_velocity.xyz);
+	_angular_velocity.copyTo(v_angular_velocity.xyz);
 	v_angular_velocity.timestamp = hrt_absolute_time();
 	_vehicle_angular_velocity_pub.publish(v_angular_velocity);
 
-				_last_publish = v_angular_velocity.timestamp_sample;
+	// shift last publish time forward, but don't let it get further behind than the interval
-				return;
+	_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
-			}
+					timestamp_sample - _publish_interval_min_us, timestamp_sample);
-		}
-	}
 }
 
 void VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d, rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]",
+	PX4_INFO("selected sensor: %d, rate: %.1f Hz %s",
-		 _calibration.device_id(), (double)_filter_sample_rate,
+		 _selected_sensor_device_id, (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "");
-		 (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
 
 	_calibration.PrintStatus();
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -36,8 +36,9 @@
 #include <lib/sensor_calibration/Gyroscope.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
-#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
-#include <lib/mathlib/math/filter/NotchFilter.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
+#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -49,6 +50,7 @@
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -72,11 +74,15 @@ public:
 private:
 	void Run() override;
 
-	void CheckAndUpdateFilters();
+	void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
+	bool UpdateSampleRate();
+
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
 
+	void Publish(const hrt_abstime &timestamp_sample);
+
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
 	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
@@ -89,32 +95,44 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
+	uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
 
 	calibration::Gyroscope _calibration{};
 
 	matrix::Vector3f _bias{};
 
-	matrix::Vector3f _angular_acceleration_prev{};
+	matrix::Vector3f _angular_velocity{};
-	matrix::Vector3f _angular_velocity_prev{};
+	matrix::Vector3f _angular_acceleration{};
-	hrt_abstime _timestamp_sample_prev{0};
 
+	matrix::Vector3f _angular_velocity_prev{};
+	hrt_abstime _timestamp_sample_last{0};
+
+	hrt_abstime _publish_interval_min_us{0};
 	hrt_abstime _last_publish{0};
+
+	float _filter_sample_rate_hz{0.f};
+
 	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
-	float _filter_sample_rate{kInitialRateHz};
 
 	// angular velocity filters
-	math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.f};
+	math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
-	math::NotchFilter<matrix::Vector3f> _notch_filter_velocity{};
+	math::NotchFilterArray<float> _notch_filter_velocity[3] {};
 
 	// angular acceleration filter
-	math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.f};
+	math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
+
+	uint32_t _selected_sensor_device_id{0};
+
+	float _fifo_last_scale{0};
+
+	bool _reset_filters{true};
+	bool _fifo_available{false};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
 		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
 		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
-
 		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
 	)
 };

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -120,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);