sensors/vehicle_angular_velocity: add IMU_GYRO_DNF_HMC to configure number of ESC RPM notch filter harmonics

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 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
@@ -57,6 +57,7 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 	perf_free(_selection_changed_perf);
 
 #if !defined(CONSTRAINED_FLASH)
+	delete[] _dynamic_notch_filter_esc_rpm;
 	perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
 	perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
 
@@ -387,11 +388,41 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 #if !defined(CONSTRAINED_FLASH)
 
 		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
-			if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
+
-				_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
+			const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10);
-						MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
+
-				_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
+			if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) {
-						MODULE_NAME": gyro dynamic notch filter ESC RPM update");
+				delete[] _dynamic_notch_filter_esc_rpm;
+				_dynamic_notch_filter_esc_rpm = nullptr;
+				_esc_rpm_harmonics = 0;
+			}
+
+			if (_dynamic_notch_filter_esc_rpm == nullptr) {
+
+				_dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics];
+
+				if (_dynamic_notch_filter_esc_rpm) {
+					_esc_rpm_harmonics = esc_rpm_harmonics;
+
+					if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
+						_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
+								MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
+					}
+
+					if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
+						_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
+								MODULE_NAME": gyro dynamic notch filter ESC RPM update");
+					}
+
+				} else {
+					_esc_rpm_harmonics = 0;
+
+					perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
+					perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
+
+					_dynamic_notch_filter_esc_rpm_disable_perf = nullptr;
+					_dynamic_notch_filter_esc_rpm_update_perf = nullptr;
+				}
 			}
 
 		} else {
@@ -452,19 +483,16 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
 {
 #if !defined(CONSTRAINED_FLASH)
 
-	if (_dynamic_notch_esc_rpm_available) {
+	if (_dynamic_notch_filter_esc_rpm) {
-		for (int axis = 0; axis < 3; axis++) {
+		for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
-			for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
+			for (int axis = 0; axis < 3; axis++) {
-				for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+				for (int esc = 0; esc < MAX_NUM_ESCS; esc++) {
-					_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].disable();
+					_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].disable();
+					_esc_available.set(esc, false);
 					perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
 				}
-
-				_esc_available.set(esc, false);
 			}
 		}
-
-		_dynamic_notch_esc_rpm_available = false;
 	}
 
 #endif // !CONSTRAINED_FLASH
@@ -491,22 +519,17 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
 void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
-	const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm;
+	const bool enabled = _dynamic_notch_filter_esc_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm);
 
 	if (enabled && (_esc_status_sub.updated() || force)) {
 
-		if (!_dynamic_notch_esc_rpm_available) {
-			// force update filters if previously disabled
-			force = true;
-		}
-
 		esc_status_s esc_status;
 
 		if (_esc_status_sub.copy(&esc_status) && (time_now_us < esc_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
 
 			static constexpr float FREQ_MIN = 10.f; // TODO: configurable
 
-			for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESC_RPM); esc++) {
+			for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) {
 				const esc_report_s &esc_report = esc_status.esc[esc];
 
 				// only update if ESC RPM range seems valid
@@ -517,17 +540,17 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
 
 					const float esc_hz = abs(esc_report.esc_rpm) / 60.f;
 
-					for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+					for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
 						const float frequency_hz = esc_hz * (harmonic + 1);
 
 						// for each ESC harmonic determine if enabled/disabled from first notch (x axis)
-						auto &nfx = _dynamic_notch_filter_esc_rpm[0][esc][harmonic];
+						auto &nfx = _dynamic_notch_filter_esc_rpm[harmonic][0][esc];
 
 						if (frequency_hz > FREQ_MIN) {
 							// update filter parameters if frequency changed or forced
 							if (update || !nfx.initialized() || (fabsf(nfx.getNotchFreq() - frequency_hz) > 0.1f)) {
 								for (int axis = 0; axis < 3; axis++) {
-									auto &nf = _dynamic_notch_filter_esc_rpm[axis][esc][harmonic];
+									auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc];
 									nf.setParameters(_filter_sample_rate_hz, frequency_hz, _param_imu_gyro_dnf_bw.get());
 									perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
 								}
@@ -539,7 +562,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
 							// disable these notch filters (if they aren't already)
 							if (nfx.getNotchFreq() > 0.f) {
 								for (int axis = 0; axis < 3; axis++) {
-									auto &nf = _dynamic_notch_filter_esc_rpm[axis][esc][harmonic];
+									auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc];
 									nf.disable();
 									perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
 								}
@@ -548,7 +571,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
 					}
 
 					if (esc_enabled) {
-						_dynamic_notch_esc_rpm_available = true;
 						_esc_available.set(esc, true);
 						_last_esc_rpm_notch_update[esc] = esc_report.timestamp;
 
@@ -560,14 +582,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
 		}
 
 		// check ESC feedback timeout
-		for (size_t esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
+		for (size_t esc = 0; esc < MAX_NUM_ESCS; esc++) {
 			if (_esc_available[esc] && (time_now_us > _last_esc_rpm_notch_update[esc] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
 
 				_esc_available.set(esc, false);
 
-				for (int axis = 0; axis < 3; axis++) {
+				for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
-					for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+					for (int axis = 0; axis < 3; axis++) {
-						_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].disable();
+						_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].disable();
 						perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
 					}
 				}
@@ -642,13 +664,11 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 #if !defined(CONSTRAINED_FLASH)
 
 	// Apply dynamic notch filter from ESC RPM
-	if (_dynamic_notch_esc_rpm_available) {
+	if (_dynamic_notch_filter_esc_rpm) {
-
+		for (int esc = 0; esc < MAX_NUM_ESCS; esc++) {
-		for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
 			if (_esc_available[esc]) {
-				// apply notch filters higher -> lowest frequency
+				for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
-				for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) {
+					_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].applyArray(data, N);
-					_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N);
 				}
 			}
 		}

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 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
@@ -143,25 +143,28 @@ private:
 
 	static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s;
 
-	static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
+	// ESC RPM
-	static constexpr int MAX_NUM_ESC_RPM_HARMONICS = 3;
+	static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
 
-	static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
+	using NotchFilterHarmonic = math::NotchFilter<float>[3][MAX_NUM_ESCS];
-				sensor_gyro_fft_s::peak_frequencies_x[0]);
+	NotchFilterHarmonic *_dynamic_notch_filter_esc_rpm{nullptr};
 
-	math::NotchFilter<float> _dynamic_notch_filter_esc_rpm[3][MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS] {};
+	int _esc_rpm_harmonics{0};
-	math::NotchFilter<float> _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {};
+	px4::Bitset<MAX_NUM_ESCS> _esc_available{};
-
+	hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {};
-	px4::Bitset<MAX_NUM_ESC_RPM> _esc_available{};
-	hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {};
 
 	perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
 
+	// FFT
+	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::NotchFilter<float> _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {};
+
 	perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
 
-	bool _dynamic_notch_esc_rpm_available{false};
 	bool _dynamic_notch_fft_available{false};
 #endif // !CONSTRAINED_FLASH
 
@@ -181,6 +184,7 @@ private:
 	DEFINE_PARAMETERS(
 #if !defined(CONSTRAINED_FLASH)
 		(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
+		(ParamInt<px4::params::IMU_GYRO_DNF_HMC>) _param_imu_gyro_dnf_hmc,
 		(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
 #endif // !CONSTRAINED_FLASH
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -147,3 +147,14 @@ PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);
 * @max 30
 */
 PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 15.f);
+
+/**
+* IMU gyro dynamic notch filter harmonics
+*
+* ESC RPM number of harmonics (multiples of RPM) for ESC RPM dynamic notch filtering.
+*
+* @group Sensors
+* @min 1
+* @max 7
+*/
+PARAM_DEFINE_INT32(IMU_GYRO_DNF_HMC, 3);