sensors: move accel filtering to sensors/vehicle_acceleration

I've added a queue depth of 4 for sensor_accel and sensor_gyro. This is initially added because it's not always possible for the `vehicle_acceleration` to keep up with every publication of the primary accelerator as it runs in the same thread as ekf2, various controllers, etc. 

Later this mechanism will be used in a few areas
 - rate limit `vehicle_angular_velocity` and `vehicle_acceleration` without missing any raw data
 - move IMU integration to `vehicle_imu` and out of the actual driver threads, eliminating the need for sensor_accel_integrated and sensor_gyro_integrated
 - integrate raw gyro synchronized with optical flow measurements

msg/sensor_accel.msg

@@ -8,3 +8,5 @@ float32 y                 # acceleration in the NED Y board axis in m/s^2
 float32 z                 # acceleration in the NED Z board axis in m/s^2
 
 float32 temperature       # temperature in degrees celsius
+
+uint8 ORB_QUEUE_LENGTH = 4

msg/sensor_accel_status.msg

@@ -11,7 +11,6 @@ uint8 rotation
 uint32[3] clipping        # clipping per axis
 
 uint16 measure_rate
-uint16 sample_rate
 
 float32 full_scale_range
 

msg/sensor_gyro.msg

@@ -8,3 +8,5 @@ float32 y                 # angular velocity in the NED Y board axis in rad/s
 float32 z                 # angular velocity in the NED Z board axis in rad/s
 
 float32 temperature       # temperature in degrees celsius
+
+uint8 ORB_QUEUE_LENGTH = 4

src/drivers/imu/adis16448/ADIS16448.cpp

@@ -258,8 +258,6 @@ ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz)
 		return false;
 	}
 
-	_px4_accel.set_sample_rate(desired_sample_rate_hz);
-
 	return true;
 }
 

src/drivers/imu/adis16477/ADIS16477.cpp

@@ -67,7 +67,6 @@ ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
 #endif // GPIO_SPI1_RESET_ADIS16477
 
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477);
-	_px4_accel.set_sample_rate(ADIS16477_DEFAULT_RATE);
 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
 
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477);

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -84,7 +84,6 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
 #endif // GPIO_SPI1_RESET_ADIS16497
 
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
-	_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
 
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
 }

src/drivers/imu/bmi160/bmi160.cpp

@@ -64,10 +64,7 @@ BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
 	_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates"))
 {
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160);
-	_px4_accel.set_sample_rate(BMI160_ACCEL_DEFAULT_RATE);
-
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160);
-	_px4_accel.set_sample_rate(BMI160_GYRO_DEFAULT_RATE);
 }
 
 BMI160::~BMI160()

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -58,8 +58,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602);
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
 
-	_px4_accel.set_sample_rate(ACCEL_RATE);
-
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
 }

src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp

@@ -58,8 +58,6 @@ ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20608);
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608);
 
-	_px4_accel.set_sample_rate(ACCEL_RATE);
-
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
 }

src/drivers/imu/lsm303d/LSM303D.cpp

@@ -363,7 +363,6 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	}
 
 	_call_accel_interval = 1000000 / accel_samplerate;
-	_px4_accel.set_sample_rate(accel_samplerate);
 
 	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
 

src/drivers/imu/st/ism330dlc/ISM330DLC.cpp

@@ -52,8 +52,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
 	_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
 
-	_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
-
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
 }

src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp

@@ -48,8 +48,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
 	_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
 
-	_px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR);
-
 	_px4_accel.set_update_rate(1000000 / _fifo_interval);
 	_px4_gyro.set_update_rate(1000000 / _fifo_interval);
 }

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -65,7 +65,6 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
 
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
-	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
 	_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
@@ -75,10 +74,6 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
 	_rotation_dcm{get_rot_matrix(rotation)}
 {
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
-
-	// set software low pass filter for controllers
-	updateParams();
-	ConfigureFilter(_param_imu_accel_cutoff.get());
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -123,13 +118,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Accelerometer::set_sample_rate(uint16_t rate)
-{
-	_sample_rate = rate;
-
-	ConfigureFilter(_filter.get_cutoff_freq());
-}
-
 void PX4Accelerometer::set_update_rate(uint16_t rate)
 {
 	const uint32_t update_interval = 1000000 / rate;
@@ -154,9 +142,20 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 	// Apply range scale and the calibrating offset/scale
 	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
 
-	// Filtered values
-	const Vector3f val_filtered{_filter.apply(val_calibrated)};
+	// publish raw data immediately
+	{
+		sensor_accel_s report{};
 
+		report.timestamp_sample = timestamp_sample;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		report.x = val_calibrated(0);
+		report.y = val_calibrated(1);
+		report.z = val_calibrated(2);
+		report.timestamp = hrt_absolute_time();
+
+		_sensor_pub.publish(report);
+	}
 
 	// Integrated values
 	Vector3f delta_velocity;
@@ -166,21 +165,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 
 	if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
 
-		// publish control data (filtered)
-		{
-			sensor_accel_s report{};
-
-			report.timestamp_sample = timestamp_sample;
-			report.device_id = _device_id;
-			report.temperature = _temperature;
-			report.x = val_filtered(0);
-			report.y = val_filtered(1);
-			report.z = val_filtered(2);
-			report.timestamp = hrt_absolute_time();
-
-			_sensor_pub.publish(report);
-		}
-
 		// fill sensor_accel_integrated and publish
 		sensor_accel_integrated_s report{};
 
@@ -211,18 +195,31 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	const uint8_t N = sample.samples;
 	const float dt = sample.dt;
 
-	// filtered data (control)
-	float x_filtered = _filterArrayX.apply(sample.x, N);
-	float y_filtered = _filterArrayY.apply(sample.y, N);
-	float z_filtered = _filterArrayZ.apply(sample.z, N);
+	// publish raw data immediately
+	{
+		// average
+		float x = (float)sum(sample.x, N) / (float)N;
+		float y = (float)sum(sample.y, N) / (float)N;
+		float z = (float)sum(sample.z, N) / (float)N;
 
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
+		// Apply rotation (before scaling)
+		rotate_3f(_rotation, x, y, z);
 
-	const Vector3f raw{x_filtered, y_filtered, z_filtered};
+		// Apply range scale and the calibrating offset/scale
+		const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
 
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)};
+		sensor_accel_s report{};
+
+		report.timestamp_sample = sample.timestamp_sample;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		report.x = val_calibrated(0);
+		report.y = val_calibrated(1);
+		report.z = val_calibrated(2);
+		report.timestamp = hrt_absolute_time();
+
+		_sensor_pub.publish(report);
+	}
 
 
 	// clipping
@@ -260,21 +257,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
 
-			// publish control data (filtered)
-			{
-				sensor_accel_s report{};
-
-				report.timestamp_sample = sample.timestamp_sample;
-				report.device_id = _device_id;
-				report.temperature = _temperature;
-				report.x = val_calibrated(0);
-				report.y = val_calibrated(1);
-				report.z = val_calibrated(2);
-				report.timestamp = hrt_absolute_time();
-
-				_sensor_pub.publish(report);
-			}
-
 			// Apply rotation and scale
 			// integrated in microseconds, convert to seconds
 			const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale};
@@ -341,7 +323,6 @@ void PX4Accelerometer::PublishStatus()
 		status.full_scale_range = _range;
 		status.rotation = _rotation;
 		status.measure_rate = _update_rate;
-		status.sample_rate = _sample_rate;
 		status.temperature = _temperature;
 		status.vibration_metric = _vibration_metric;
 		status.clipping[0] = _clipping[0];
@@ -364,15 +345,6 @@ void PX4Accelerometer::ResetIntegrator()
 	_timestamp_sample_prev = 0;
 }
 
-void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
-{
-	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
-
-	_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
-	_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
-	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
-}
-
 void PX4Accelerometer::UpdateClipLimit()
 {
 	// 95% of potential max
@@ -391,8 +363,6 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
 void PX4Accelerometer::print_status()
 {
 	PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-	PX4_INFO("sample rate: %d Hz", _sample_rate);
-	PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq());
 
 	PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
 		 (double)_calibration_scale(2));

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -39,16 +39,14 @@
 #include <lib/conversion/rotation.h>
 #include <lib/drivers/device/integrator.h>
 #include <lib/ecl/geo/geo.h>
-#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
-#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
-#include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
+#include <uORB/PublicationQueuedMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 #include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_accel_status.h>
 
-class PX4Accelerometer : public cdev::CDev, public ModuleParams
+class PX4Accelerometer : public cdev::CDev
 {
 public:
 	PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -62,7 +60,6 @@ public:
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint64_t error_count) { _error_count += error_count; }
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
-	void set_sample_rate(uint16_t rate);
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
@@ -88,25 +85,18 @@ public:
 
 private:
 
-	void ConfigureFilter(float cutoff_freq);
 	void PublishStatus();
 	void ResetIntegrator();
 	void UpdateClipLimit();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
 
-	uORB::PublicationMulti<sensor_accel_s>            _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_accel_s>      _sensor_pub;
 	uORB::PublicationMulti<sensor_accel_fifo_s>       _sensor_fifo_pub;
 	uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
 	uORB::PublicationMulti<sensor_accel_status_s>     _sensor_status_pub;
 
-	math::LowPassFilter2pVector3f _filter{1000, 100};
-
 	hrt_abstime	_status_last_publish{0};
 
-	math::LowPassFilter2pArray _filterArrayX{4000, 100};
-	math::LowPassFilter2pArray _filterArrayY{4000, 100};
-	math::LowPassFilter2pArray _filterArrayZ{4000, 100};
-
 	Integrator		_integrator{4000, false};
 
 	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
@@ -131,7 +121,6 @@ private:
 
 	uint32_t		_clipping[3] {};
 
-	uint16_t		_sample_rate{1000};
 	uint16_t		_update_rate{1000};
 
 	// integrator
@@ -143,7 +132,4 @@ private:
 	uint8_t			_integrator_fifo_samples{0};
 	uint8_t			_integrator_clipping{0};
 
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
-	)
 };

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -39,6 +39,7 @@
 #include <lib/conversion/rotation.h>
 #include <lib/drivers/device/integrator.h>
 #include <uORB/PublicationMulti.hpp>
+#include <uORB/PublicationQueuedMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_gyro_integrated.h>
@@ -88,7 +89,7 @@ private:
 	void UpdateClipLimit();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
 
-	uORB::PublicationMulti<sensor_gyro_s>            _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_gyro_s>      _sensor_pub;
 	uORB::PublicationMulti<sensor_gyro_fifo_s>       _sensor_fifo_pub;
 	uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
 	uORB::PublicationMulti<sensor_gyro_status_s>     _sensor_status_pub;

src/modules/sensors/sensor_params.c

@@ -212,17 +212,3 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
-
-/**
-* Driver level cutoff frequency for accel
-*
-* The cutoff frequency for the 2nd order butterworth filter on the accel driver.
-* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
-*
-* @min 0
-* @max 1000
-* @unit Hz
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -43,11 +43,14 @@ VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
 {
+	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
 }
 
 VehicleAcceleration::~VehicleAcceleration()
 {
 	Stop();
+
+	perf_free(_interval_perf);
 }
 
 bool VehicleAcceleration::Start()
@@ -77,6 +80,45 @@ void VehicleAcceleration::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
+void VehicleAcceleration::CheckFilters()
+{
+	if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
+		_filter_check_last = hrt_absolute_time();
+
+		// calculate sensor update rate
+		const float sample_interval_avg = perf_mean(_interval_perf);
+
+		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
+
+			const float update_rate_hz = 1.0f / sample_interval_avg;
+
+			if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
+				_update_rate_hz = update_rate_hz;
+
+				// check if sample rate error is greater than 1%
+				if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
+					++_sample_rate_incorrect_count;
+				}
+			}
+		}
+
+		const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
+		const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f);
+
+		if (sample_rate_updated || lp_updated) {
+			PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
+			_filter_sample_rate = _update_rate_hz;
+
+			// update software low pass filters
+			_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
+			_lp_filter.reset(_acceleration_prev);
+
+			// reset state
+			_sample_rate_incorrect_count = 0;
+		}
+	}
+}
+
 void VehicleAcceleration::SensorBiasUpdate(bool force)
 {
 	if (_estimator_sensor_bias_sub.updated() || force) {
@@ -165,6 +207,9 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 						// force corrections reselection
 						_corrections_selected_instance = -1;
 
+						// reset sample rate monitor
+						_sample_rate_incorrect_count = 0;
+
 						return true;
 					}
 				}
@@ -205,35 +250,56 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 void VehicleAcceleration::Run()
 {
 	// update corrections first to set _selected_sensor
-	bool sensor_select_update = SensorSelectionUpdate();
-	SensorCorrectionsUpdate(sensor_select_update);
-	SensorBiasUpdate(sensor_select_update);
+	bool selection_updated = SensorSelectionUpdate();
+
+	SensorCorrectionsUpdate(selection_updated);
+	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
-	if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
+	bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
+
+	// process all outstanding messages
+	while (sensor_updated || selection_updated) {
+		selection_updated = false;
+
 		sensor_accel_s sensor_data;
 
 		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
-			// get the sensor data and correct for thermal errors (apply offsets and scale)
-			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
-			// apply offsets and scale
-			Vector3f accel{(val - _offset).emult(_scale)};
+			if (sensor_updated) {
+				perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
+			}
 
-			// rotate corrected measurements from sensor to body frame
-			accel = _board_rotation * accel;
+			CheckFilters();
 
-			// correct for in-run bias errors
-			accel -= _bias;
+			// Filter: apply low-pass
+			const Vector3f accel_filtered = _lp_filter.apply(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z});
 
-			// publish
-			vehicle_acceleration_s out;
+			_acceleration_prev = accel_filtered;
 
-			out.timestamp_sample = sensor_data.timestamp_sample;
-			accel.copyTo(out.xyz);
-			out.timestamp = hrt_absolute_time();
+			// publish once all new samples are processed
+			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
 
-			_vehicle_acceleration_pub.publish(out);
+			if (!sensor_updated) {
+				// apply offsets and scale
+				Vector3f accel{(accel_filtered - _offset).emult(_scale)};
+
+				// rotate corrected measurements from sensor to body frame
+				accel = _board_rotation * accel;
+
+				// correct for in-run bias errors
+				accel -= _bias;
+
+				// Publish vehicle_acceleration
+				vehicle_acceleration_s v_acceleration;
+				v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
+				accel.copyTo(v_acceleration.xyz);
+				v_acceleration.timestamp = hrt_absolute_time();
+				_vehicle_acceleration_pub.publish(v_acceleration);
+
+				_last_publish = v_acceleration.timestamp_sample;
+				return;
+			}
 		}
 	}
 }
@@ -244,4 +310,7 @@ void VehicleAcceleration::PrintStatus()
 	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
 	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
 	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
+
+	PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
+	PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter.get_cutoff_freq());
 }

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -36,6 +36,7 @@
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -65,6 +66,7 @@ public:
 private:
 	void Run() override;
 
+	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	void SensorCorrectionsUpdate(bool force = false);
@@ -73,6 +75,8 @@ private:
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
+
 		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
@@ -82,8 +86,8 @@ private:
 
 	uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
-	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
@@ -93,12 +97,26 @@ private:
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+
 	matrix::Dcmf _board_rotation;
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 	matrix::Vector3f _offset{0.f, 0.f, 0.f};
 	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
+	matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
+
+	hrt_abstime _last_publish{0};
+	hrt_abstime _filter_check_last{0};
+	static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
+	float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
+
+	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
+
+	float _filter_sample_rate{kInitialRateHz};
+	int _sample_rate_incorrect_count{0};
+
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
 	int8_t _corrections_selected_instance{-1};

src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c

@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+* Low pass filter cutoff frequency for accel
+*
+* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer.
+* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -273,7 +273,10 @@ void VehicleAngularVelocity::Run()
 
 	bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
 
-	if (sensor_updated || selection_updated) {
+	// process all outstanding messages
+	while (sensor_updated || selection_updated) {
+		selection_updated = false;
+
 		sensor_gyro_s sensor_data;
 
 		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
@@ -286,12 +289,8 @@ void VehicleAngularVelocity::Run()
 			const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f);
 			_timestamp_sample_prev = sensor_data.timestamp_sample;
 
-
 			// get the sensor data and correct for thermal errors (apply offsets and scale)
-			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
-
-			// apply offsets and scale
-			Vector3f angular_velocity_raw{(val - _offset).emult(_scale)};
+			Vector3f angular_velocity_raw{(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z} - _offset).emult(_scale)};
 
 			// rotate corrected measurements from sensor to body frame
 			angular_velocity_raw = _board_rotation * angular_velocity_raw;
@@ -312,32 +311,38 @@ void VehicleAngularVelocity::Run()
 			const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
 			const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
 
-			bool publish = true;
+			// publish once all new samples are processed
+			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
 
-			if (_param_imu_gyro_rate_max.get() > 0) {
-				const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
+			if (!sensor_updated) {
+				bool publish = true;
 
-				if (hrt_elapsed_time(&_last_publish) < interval) {
-					publish = false;
+				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) {
-				// Publish vehicle_angular_acceleration
-				vehicle_angular_acceleration_s v_angular_acceleration;
-				v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
-				angular_acceleration.copyTo(v_angular_acceleration.xyz);
-				v_angular_acceleration.timestamp = hrt_absolute_time();
-				_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
+				if (publish) {
+					// Publish vehicle_angular_acceleration
+					vehicle_angular_acceleration_s v_angular_acceleration;
+					v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
+					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;
-				angular_velocity.copyTo(v_angular_velocity.xyz);
-				v_angular_velocity.timestamp = hrt_absolute_time();
-				_vehicle_angular_velocity_pub.publish(v_angular_velocity);
+					// Publish vehicle_angular_velocity
+					vehicle_angular_velocity_s v_angular_velocity;
+					v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
+					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;
+					_last_publish = v_angular_velocity.timestamp_sample;
+					return;
+				}
 			}
 		}
 	}

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -36,7 +36,6 @@
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
-#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <lib/mathlib/math/filter/NotchFilter.hpp>
 #include <px4_platform_common/log.h>
@@ -95,8 +94,8 @@ private:
 	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
-	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
-* Driver level notch frequency for gyro
+* Notch filter frequency for gyro
 *
-* The center frequency for the 2nd order notch filter on the gyro driver.
+* The center frequency for the 2nd order notch filter on the primary gyro.
 * This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
 * This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
 * See "IMU_GYRO_NF_BW" to set the bandwidth of the filter.
@@ -48,9 +48,9 @@
 PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
 
 /**
-* Driver level notch bandwidth for gyro
+* Notch filter bandwidth for gyro
 *
-* The frequency width of the stop band for the 2nd order notch filter on the gyro driver.
+* The frequency width of the stop band for the 2nd order notch filter on the primary gyro.
 * See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
 *
 * @min 0
@@ -62,9 +62,9 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
 PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
 
 /**
-* Driver level cutoff frequency for gyro
+* Low pass filter cutoff frequency for gyro
 *
-* The cutoff frequency for the 2nd order butterworth filter on the gyro driver.
+* The cutoff frequency for the 2nd order butterworth filter on the primary gyro.
 * This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
 *
 * @min 0

src/modules/simulator/simulator.h

@@ -103,7 +103,6 @@ public:
 private:
 	Simulator() : ModuleParams(nullptr)
 	{
-		_px4_accel.set_sample_rate(250);
 	}
 
 	~Simulator()

src/modules/uORB/CMakeLists.txt

@@ -44,6 +44,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 			Publication.hpp
 			PublicationMulti.hpp
 			PublicationQueued.hpp
+			PublicationQueuedMulti.hpp
 			Subscription.cpp
 			Subscription.hpp
 			SubscriptionCallback.hpp

src/modules/uORB/PublicationQueuedMulti.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 PublicationQueuedMulti.hpp
+ *
+ */
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+
+namespace uORB
+{
+
+/**
+ * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH)
+ */
+template<typename T>
+class PublicationQueuedMulti
+{
+public:
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 */
+	PublicationQueuedMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) :
+		_meta(meta),
+		_priority(priority)
+	{}
+
+	~PublicationQueuedMulti()
+	{
+		//orb_unadvertise(_handle);
+	}
+
+	/**
+	 * Publish the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	bool publish(const T &data)
+	{
+		if (_handle != nullptr) {
+			return (orb_publish(_meta, _handle, &data) == PX4_OK);
+
+		} else {
+			int instance = 0;
+			orb_advert_t handle = orb_advertise_multi_queue(_meta, &data, &instance, _priority, T::ORB_QUEUE_LENGTH);
+
+			if (handle != nullptr) {
+				_handle = handle;
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+protected:
+	const orb_metadata *_meta;
+
+	orb_advert_t _handle{nullptr};
+
+	const uint8_t _priority;
+};
+
+} // namespace uORB