sensors/vehicle_angular_velocity: gyro dynamic notch filters updated from onboard FFT

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -151,6 +151,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
 		// 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));
+
+		// dynamic notch filters, first disable, then force update (if available)
+		DisableDynamicNotchFFT();
+		UpdateDynamicNotchFFT(true);
 	}
 
 	_reset_filters = false;
@@ -281,6 +285,68 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
+void VehicleAngularVelocity::DisableDynamicNotchFFT()
+{
+#if !defined(CONSTRAINED_FLASH)
+
+	// device id mismatch, disable all
+	for (auto &dnf : _dynamic_notch_filter) {
+		for (int axis = 0; axis < 3; axis++) {
+			dnf[axis].setParameters(0, 0, 0);
+		}
+	}
+
+	_dynamic_notch_available = false;
+#endif // !CONSTRAINED_FLASH
+}
+
+void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
+{
+#if !defined(CONSTRAINED_FLASH)
+
+	if (_param_imu_gyro_dyn_nf.get() && (_sensor_gyro_fft_sub.updated() || force)) {
+		sensor_gyro_fft_s sensor_gyro_fft;
+
+		if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id)
+		    && (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 0.05f)) {
+
+			float *peak_frequencies[] { sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z};
+
+			for (int axis = 0; axis < 3; axis++) {
+				for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) {
+					auto &dnf = _dynamic_notch_filter[i][axis];
+					const float &peak_freq = peak_frequencies[axis][i];
+
+					if (PX4_ISFINITE(peak_freq) && (peak_freq > 1.f)) {
+						const float change_percent = fabsf(dnf.getNotchFreq() - peak_freq) / peak_freq;
+
+						if (change_percent > 0.001f) {
+							// peak frequency changed by at least 0.1%
+							dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz);
+
+							// only reset if there's sufficient change (> 1%)
+							if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) {
+								dnf.reset(_angular_velocity(axis) / _fifo_last_scale);
+							}
+						}
+
+						_dynamic_notch_available = true;
+
+					} else {
+						// disable this notch filter
+						dnf.setParameters(0, 0, 0);
+					}
+				}
+			}
+
+		} else {
+			DisableDynamicNotchFFT();
+		}
+	}
+
+#endif // !CONSTRAINED_FLASH
+}
+
 void VehicleAngularVelocity::Run()
 {
 	// backup schedule
@@ -319,6 +385,8 @@ void VehicleAngularVelocity::Run()
 					}
 				}
 
+				UpdateDynamicNotchFFT();
+
 				Vector3f angular_velocity_unscaled;
 				Vector3f angular_acceleration_unscaled;
 
@@ -332,6 +400,19 @@ void VehicleAngularVelocity::Run()
 						data[n] = raw_data_array[axis][n];
 					}
 
+#if !defined(CONSTRAINED_FLASH)
+
+					// Apply dynamic notch filter from FFT
+					if (_dynamic_notch_available) {
+						for (auto &dnf : _dynamic_notch_filter) {
+							if (dnf[axis].getNotchFreq() > 0.f) {
+								dnf[axis].applyDF1(data, N);
+							}
+						}
+					}
+
+#endif // !CONSTRAINED_FLASH
+
 					// Apply general notch filter (IMU_GYRO_NF_FREQ)
 					if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
 						_notch_filter_velocity[axis].apply(data, N);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -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
@@ -50,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_fft.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
@@ -66,22 +67,21 @@ public:
 	VehicleAngularVelocity();
 	~VehicleAngularVelocity() override;
 
+	void PrintStatus();
 	bool Start();
 	void Stop();
 
-	void PrintStatus();
-
 private:
 	void Run() override;
 
-	void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
-	bool UpdateSampleRate();
-
+	void DisableDynamicNotchFFT();
 	void ParametersUpdate(bool force = false);
+	void Publish(const hrt_abstime &timestamp_sample);
+	void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
-
-	void Publish(const hrt_abstime &timestamp_sample);
+	void UpdateDynamicNotchFFT(bool force = false);
+	bool UpdateSampleRate();
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
@@ -90,6 +90,9 @@ private:
 
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
+#if !defined(CONSTRAINED_FLASH)
+	uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
+#endif // !CONSTRAINED_FLASH
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -118,6 +121,14 @@ private:
 	math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
 	math::NotchFilterArray<float> _notch_filter_velocity[3] {};
 
+#if !defined(CONSTRAINED_FLASH)
+	static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
+				sensor_gyro_fft_s::peak_frequencies_x[0]);
+
+	math::NotchFilterArray<float> _dynamic_notch_filter[MAX_NUM_FFT_PEAKS][3] {};
+	bool _dynamic_notch_available{false};
+#endif // !CONSTRAINED_FLASH
+
 	// angular acceleration filter
 	math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
 
@@ -133,7 +144,8 @@ private:
 		(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_ratemax,
-		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
+		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
+		(ParamBool<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf
 	)
 };
 

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -121,3 +121,15 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400);
 * @group Sensors
 */
 PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
+
+/**
+* IMU gyro dynamic notch filtering
+*
+* Enable bank of dynamically updating notch filters.
+* Requires onboard FFT (IMU_GYRO_FFT_EN).
+*
+* @boolean
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);