sensors: move gyro filtering to sensors/vehicle_angular_velocity

- gyro filtering (low-pass and notch) only performed on primary gyro in `sensors/vehicle_angular_velocity` instead of every gyro in `PX4Gyroscope`
 - sample rate is calculated from actual updates (the fixed value was slightly wrong in many cases, and very wrong in a few)
 - In the FIFO case the array is now averaged and published in `sensor_gyro` for filtering downstream. I'll update this in the future to use the full FIFO array (if available), but right now it should be fine.

src/modules/sensors/sensor_params.c

@@ -213,70 +213,6 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
  */
 PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
 
-/**
-* Driver level notch frequency for gyro
-*
-* The center frequency for the 2nd order notch filter on the gyro driver.
-* 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.
-*
-* @min 0
-* @max 1000
-* @unit Hz
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
-
-/**
-* Driver level notch bandwidth for gyro
-*
-* The frequency width of the stop band for the 2nd order notch filter on the gyro driver.
-* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
-*
-* @min 0
-* @max 100
-* @unit Hz
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
-
-/**
-* Driver level cutoff frequency for gyro
-*
-* The cutoff frequency for the 2nd order butterworth filter on the gyro 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_GYRO_CUTOFF, 30.0f);
-
-/**
-* Gyro control data maximum publication rate
-*
-* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
-* Set to 0 to disable and publish at the native sensor sample rate.
-*
-* @min 0
-* @max 2000
-* @value 0 0 (no limit)
-* @value 50 50 Hz
-* @value 250 250 Hz
-* @value 400 400 Hz
-* @value 1000 1000 Hz
-* @value 2000 2000 Hz
-* @unit Hz
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
-
 /**
 * Driver level cutoff frequency for accel
 *

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -43,11 +43,15 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
+	_lowpass_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
+	_notch_filter.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()
 {
 	Stop();
+
+	perf_free(_interval_perf);
 }
 
 bool VehicleAngularVelocity::Start()
@@ -77,6 +81,50 @@ void VehicleAngularVelocity::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
+void VehicleAngularVelocity::CheckFilters(const Vector3f &rates)
+{
+	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 lowpass_updated = (fabsf(_lowpass_filter.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
+		const bool notch_updated = ((fabsf(_notch_filter.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
+					    || (fabsf(_notch_filter.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
+
+		if (sample_rate_updated || lowpass_updated || notch_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
+			_lowpass_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
+			_lowpass_filter.reset(rates);
+
+			_notch_filter.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+			_notch_filter.reset(rates);
+
+			// reset state
+			_sample_rate_incorrect_count = 0;
+		}
+	}
+}
+
 void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 {
 	if (_estimator_sensor_bias_sub.updated() || force) {
@@ -165,6 +213,9 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						// force corrections reselection
 						_corrections_selected_instance = -1;
 
+						// reset sample rate monitor
+						_sample_rate_incorrect_count = 0;
+
 						return true;
 					}
 				}
@@ -205,15 +256,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 void VehicleAngularVelocity::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();
+
+	if (sensor_updated || selection_updated) {
 		sensor_gyro_s sensor_data;
 
 		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
+
+			if (sensor_updated) {
+				perf_count_interval(_interval_perf, 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};
 
@@ -226,14 +285,32 @@ void VehicleAngularVelocity::Run()
 			// correct for in-run bias errors
 			rates -= _bias;
 
-			// publish
-			vehicle_angular_velocity_s out;
+			// Filter: apply notch and then low-pass
+			CheckFilters(rates);
+			const Vector3f rates_filtered = _lowpass_filter.apply(_notch_filter.apply(rates));
 
-			out.timestamp_sample = sensor_data.timestamp_sample;
-			rates.copyTo(out.xyz);
-			out.timestamp = hrt_absolute_time();
 
-			_vehicle_angular_velocity_pub.publish(out);
+			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) {
+				vehicle_angular_velocity_s out;
+
+				out.timestamp_sample = sensor_data.timestamp_sample;
+				rates_filtered.copyTo(out.xyz);
+				out.timestamp = hrt_absolute_time();
+
+				_vehicle_angular_velocity_pub.publish(out);
+
+				_last_publish = out.timestamp_sample;
+			}
 		}
 	}
 }
@@ -244,4 +321,12 @@ void VehicleAngularVelocity::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)_lowpass_filter.get_cutoff_freq());
+
+	if (_notch_filter.getNotchFreq() > 0.0f) {
+		PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
+			 (double)_notch_filter.getBandwidth());
+	}
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -36,6 +36,9 @@
 #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>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -65,6 +68,7 @@ public:
 private:
 	void Run() override;
 
+	void CheckFilters(const matrix::Vector3f &rates);
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	void SensorCorrectionsUpdate(bool force = false);
@@ -73,6 +77,11 @@ private:
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
 	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::SENS_BOARD_ROT>) _param_sens_board_rot,
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
@@ -93,12 +102,23 @@ private:
 		{this, ORB_ID(sensor_gyro), 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};
 
+	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 _lowpass_filter{kInitialRateHz, 30};
+	math::NotchFilter<matrix::Vector3f> _notch_filter{};
+	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_angular_velocity/imu_gyro_parameters.c

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+* Driver level notch frequency for gyro
+*
+* The center frequency for the 2nd order notch filter on the gyro driver.
+* 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.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
+
+/**
+* Driver level notch bandwidth for gyro
+*
+* The frequency width of the stop band for the 2nd order notch filter on the gyro driver.
+* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
+*
+* @min 0
+* @max 100
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
+
+/**
+* Driver level cutoff frequency for gyro
+*
+* The cutoff frequency for the 2nd order butterworth filter on the gyro 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_GYRO_CUTOFF, 30.0f);
+
+/**
+* Gyro control data maximum publication rate
+*
+* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
+* Set to 0 to disable and publish at the native sensor sample rate.
+*
+* @min 0
+* @max 2000
+* @value 0 0 (no limit)
+* @value 50 50 Hz
+* @value 250 250 Hz
+* @value 400 400 Hz
+* @value 1000 1000 Hz
+* @value 2000 2000 Hz
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);

src/modules/simulator/simulator.h

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