Add gyroscope notch filter - IMU_GYRO_NF_(FREQ|BW)

* NotchFilter: add NotchFilter template and test for float type
* NotchFilterTest: add test for Vector3f notch filter
* PX4Gyroscope: add notch filter with IMU_GYRO_NF_(FREQ|BW) parameters to
set the notch center frequency and bandwidth

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -54,6 +54,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	// set software low pass filter for controllers
 	updateParams();
 	ConfigureFilter(_param_imu_gyro_cutoff.get());
+	ConfigureNotchFilter(_param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -105,6 +106,7 @@ PX4Gyroscope::set_sample_rate(uint16_t rate)
 	_sample_rate = rate;
 
 	ConfigureFilter(_filter.get_cutoff_freq());
+	ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth());
 }
 
 void
@@ -137,9 +139,9 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 	// Apply range scale and the calibrating offset/scale
 	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
 
-	// Filtered values
-	const Vector3f val_filtered{_filter.apply(val_calibrated)};
-
+	// Filtered values: apply notch and then low-pass
+	Vector3f val_filtered{_notch_filter.apply(val_calibrated)};
+	val_filtered = _filter.apply(val_filtered);
 
 	// publish control data (filtered) immediately
 	bool publish_control = true;
@@ -428,6 +430,12 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq)
 	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
 }
 
+void
+PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
+{
+	_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
+}
+
 void
 PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 {
@@ -448,6 +456,8 @@ PX4Gyroscope::print_status()
 	PX4_INFO(GYRO_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("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
+		 (double)_notch_filter.getBandwidth());
 
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -40,6 +40,7 @@
 #include <lib/drivers/device/integrator.h>
 #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/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
@@ -89,6 +90,7 @@ public:
 private:
 
 	void ConfigureFilter(float cutoff_freq);
+	void ConfigureNotchFilter(float notch_freq, float bandwidth);
 	void ResetIntegrator();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
 
@@ -98,6 +100,7 @@ private:
 	uORB::PublicationMultiData<sensor_gyro_status_s>	_sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
+	math::NotchFilter<matrix::Vector3f> _notch_filter{};
 
 	hrt_abstime	_control_last_publish{0};
 
@@ -140,6 +143,8 @@ private:
 
 	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
 	)