feat(dynamic-notch-filter): support for RPM input for notch filter

src/drivers/rpm/rpm_simulator/rpm_simulator.cpp

@@ -36,14 +36,19 @@
  * @file rpm_simulator.cpp
  * Simple app for publishing RPM messages with custom value.
  *
- * Usage: rpm_simulator <rpm_value>
+ * Usage: rpm_simulator <rpm_value> [duration_s]
  *  rpm_simulator 344.2
+ *  rpm_simulator 344.2 10   # keep republishing for 10s (uORB::Publication
+ *                           # unadvertises on destruction, so a single-shot
+ *                           # publish disappears again as soon as this
+ *                           # command returns)
  *
  * @author ThunderFly s.r.o., Roman Dvorak <dvorakroman@thunderfly.cz>
  */
 
 #include <px4_platform_common/px4_config.h>
 #include <drivers/drv_hrt.h>
+#include <px4_platform_common/posix.h>
 
 #include <uORB/Publication.hpp>
 #include <uORB/topics/rpm.h>
@@ -52,25 +57,35 @@ extern "C" __EXPORT int rpm_simulator_main(int argc, char *argv[]);
 int rpm_simulator_main(int argc, char *argv[])
 {
 	// check input
-	if (argc != 2) {
-		PX4_INFO("Usage: rpm_simulator <published RPM>");
+	if (argc < 2) {
+		PX4_INFO("Usage: rpm_simulator <published RPM> [duration_s]");
 		PX4_INFO("Exit. Without publishing any message.");
 		return 0;
 	}
 
 	rpm_s rpm{};
 
 	uORB::Publication<rpm_s> rpm_pub{ORB_ID(rpm)};
-	uint64_t timestamp_us = hrt_absolute_time();
 	float frequency = atof(argv[1]);
+	float duration_s = (argc >= 3) ? (float)atof(argv[2]) : 0.f;
 
 	// prpepare RPM data message
-	rpm.timestamp = timestamp_us;
+	rpm.timestamp = hrt_absolute_time();
 	rpm.rpm_estimate = frequency;
 
 	// Publish data and let the user know what was published
 	rpm_pub.publish(rpm);
 	print_message(ORB_ID(rpm), rpm);
 
+	if (duration_s > 0.f) {
+		const hrt_abstime end = hrt_absolute_time() + (hrt_abstime)(duration_s * 1e6f);
+
+		while (hrt_absolute_time() < end) {
+			px4_usleep(100000);
+			rpm.timestamp = hrt_absolute_time();
+			rpm_pub.publish(rpm);
+		}
+	}
+
 	return 0;
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -63,6 +63,12 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 	perf_free(_dynamic_notch_filter_esc_rpm_init_perf);
 	perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
 
+	delete[] _dynamic_notch_filter_rotor_rpm;
+	perf_free(_dynamic_notch_filter_rotor_rpm_disable_perf);
+	perf_free(_dynamic_notch_filter_rotor_rpm_init_perf);
+	perf_free(_dynamic_notch_filter_rotor_rpm_update_perf);
+
+
 	perf_free(_dynamic_notch_filter_fft_disable_perf);
 	perf_free(_dynamic_notch_filter_fft_update_perf);
 #endif // CONSTRAINED_FLASH
@@ -195,6 +201,7 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
 
 		// force reset notch filters on any scale change
 		UpdateDynamicNotchEscRpm(time_now_us, true);
+		UpdateDynamicNotchRotorRpm(time_now_us, true);
 		UpdateDynamicNotchFFT(time_now_us, true);
 
 		_angular_velocity_raw_prev = angular_velocity_uncalibrated;
@@ -502,6 +509,56 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 			DisableDynamicNotchFFT();
 		}
 
+		// Rotor RPM dynamic notches: use same harmonics parameter for now
+		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::RotorRpm) {
+
+			const int32_t rotor_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10);
+
+			if (_dynamic_notch_filter_rotor_rpm && (rotor_rpm_harmonics != _rotor_rpm_harmonics)) {
+				delete[] _dynamic_notch_filter_rotor_rpm;
+				_dynamic_notch_filter_rotor_rpm = nullptr;
+				_rotor_rpm_harmonics = 0;
+			}
+
+			if (_dynamic_notch_filter_rotor_rpm == nullptr) {
+
+				_dynamic_notch_filter_rotor_rpm = new NotchFilterRotorRpmHarmonic[rotor_rpm_harmonics];
+
+				if (_dynamic_notch_filter_rotor_rpm) {
+					_rotor_rpm_harmonics = rotor_rpm_harmonics;
+
+					if (_dynamic_notch_filter_rotor_rpm_disable_perf == nullptr) {
+						_dynamic_notch_filter_rotor_rpm_disable_perf = perf_alloc(PC_COUNT,
+								MODULE_NAME": gyro dynamic notch filter Rotor RPM disable");
+					}
+
+					if (_dynamic_notch_filter_rotor_rpm_init_perf == nullptr) {
+						_dynamic_notch_filter_rotor_rpm_init_perf = perf_alloc(PC_COUNT,
+								MODULE_NAME": gyro dynamic notch filter Rotor RPM init");
+					}
+
+					if (_dynamic_notch_filter_rotor_rpm_update_perf == nullptr) {
+						_dynamic_notch_filter_rotor_rpm_update_perf = perf_alloc(PC_COUNT,
+								MODULE_NAME": gyro dynamic notch filter Rotor RPM update");
+					}
+
+				} else {
+					_rotor_rpm_harmonics = 0;
+
+					perf_free(_dynamic_notch_filter_rotor_rpm_disable_perf);
+					perf_free(_dynamic_notch_filter_rotor_rpm_init_perf);
+					perf_free(_dynamic_notch_filter_rotor_rpm_update_perf);
+
+					_dynamic_notch_filter_rotor_rpm_disable_perf = nullptr;
+					_dynamic_notch_filter_rotor_rpm_init_perf = nullptr;
+					_dynamic_notch_filter_rotor_rpm_update_perf = nullptr;
+				}
+			}
+
+		} else {
+			DisableDynamicNotchRotorRpm();
+		}
+
 #endif // !CONSTRAINED_FLASH
 	}
 }
@@ -573,6 +630,25 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
 #endif // !CONSTRAINED_FLASH
 }
 
+void VehicleAngularVelocity::DisableDynamicNotchRotorRpm()
+{
+#if !defined(CONSTRAINED_FLASH)
+
+	if (_dynamic_notch_filter_rotor_rpm) {
+		for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) {
+			for (int axis = 0; axis < 3; axis++) {
+				for (int sensor = 0; sensor < MAX_NUM_ROTOR_RPM_SENSORS; sensor++) {
+					_dynamic_notch_filter_rotor_rpm[harmonic][axis][sensor].disable();
+					_rotor_rpm_available.set(sensor, false);
+					perf_count(_dynamic_notch_filter_rotor_rpm_disable_perf);
+				}
+			}
+		}
+	}
+
+#endif // !CONSTRAINED_FLASH
+}
+
 void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
@@ -729,6 +805,91 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(const hrt_abstime &time_now_u
 #endif // !CONSTRAINED_FLASH
 }
 
+void VehicleAngularVelocity::UpdateDynamicNotchRotorRpm(const hrt_abstime &time_now_us, bool force)
+{
+#if !defined(CONSTRAINED_FLASH)
+	const bool enabled = _dynamic_notch_filter_rotor_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::RotorRpm);
+
+	if (enabled) {
+		bool axis_init[3] {false, false, false};
+
+		const float bandwidth_hz = _param_imu_gyro_dnf_bw.get();
+		const float freq_min = math::max(_param_imu_gyro_dnf_min.get(), bandwidth_hz);
+
+		for (unsigned i = 0; i < MAX_NUM_ROTOR_RPM_SENSORS; i++) {
+			rpm_s rpm_status{};
+
+			if (_rpm_sub[i].copy(&rpm_status) &&
+			    (time_now_us < rpm_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
+
+				// rpm_estimate of 0 means no movement measured within the driver's own timeout
+				if (rpm_status.rpm_estimate > FLT_EPSILON) {
+					const float rotor_hz = rpm_status.rpm_estimate / 60.f;
+					const bool force_update = force || !_rotor_rpm_available[i];
+
+					for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) {
+						const float frequency_hz = math::max(rotor_hz * (harmonic + 1),
+										     freq_min + (harmonic * 0.5f * bandwidth_hz));
+
+						for (int axis = 0; axis < 3; axis++) {
+							auto &nf = _dynamic_notch_filter_rotor_rpm[harmonic][axis][i];
+
+							const float notch_freq_delta = fabsf(nf.getNotchFreq() - frequency_hz);
+							const bool notch_freq_changed = (notch_freq_delta > 0.1f);
+							const bool allow_update = !axis_init[axis] || (nf.initialized() && notch_freq_delta < nf.getBandwidth());
+
+							if ((force_update || notch_freq_changed) && allow_update) {
+								if (nf.setParameters(_filter_sample_rate_hz, frequency_hz, bandwidth_hz)) {
+									perf_count(_dynamic_notch_filter_rotor_rpm_update_perf);
+
+									if (!nf.initialized()) {
+										perf_count(_dynamic_notch_filter_rotor_rpm_init_perf);
+										axis_init[axis] = true;
+									}
+								}
+							}
+						}
+					}
+
+					_rotor_rpm_available.set(i, true);
+					_last_rotor_rpm_notch_update[i] = rpm_status.timestamp;
+				}
+			}
+		}
+
+		// timeout handling
+		for (unsigned i = 0; i < MAX_NUM_ROTOR_RPM_SENSORS; i++) {
+			if (_rotor_rpm_available[i] && (time_now_us > _last_rotor_rpm_notch_update[i] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
+				bool all_disabled = true;
+
+				for (int harmonic = _rotor_rpm_harmonics - 1; harmonic >= 0; harmonic--) {
+					for (int axis = 0; axis < 3; axis++) {
+						auto &nf = _dynamic_notch_filter_rotor_rpm[harmonic][axis][i];
+
+						if (nf.getNotchFreq() > 0.f) {
+							if (nf.initialized() && !axis_init[axis]) {
+								nf.disable();
+								perf_count(_dynamic_notch_filter_rotor_rpm_disable_perf);
+								axis_init[axis] = true;
+							}
+						}
+
+						if (nf.getNotchFreq() > 0.f) {
+							all_disabled = false;
+						}
+					}
+				}
+
+				if (all_disabled) {
+					_rotor_rpm_available.set(i, false);
+				}
+			}
+		}
+	}
+
+#endif // !CONSTRAINED_FLASH
+}
+
 float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N)
 {
 #if !defined(CONSTRAINED_FLASH)
@@ -746,6 +907,19 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 		}
 	}
 
+	// Apply dynamic notch filter from Rotor RPM
+	if (_dynamic_notch_filter_rotor_rpm) {
+		for (int inst = 0; inst < MAX_NUM_ROTOR_RPM_SENSORS; inst++) {
+			if (_rotor_rpm_available[inst]) {
+				for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) {
+					if (_dynamic_notch_filter_rotor_rpm[harmonic][axis][inst].getNotchFreq() > 0.f) {
+						_dynamic_notch_filter_rotor_rpm[harmonic][axis][inst].applyArray(data, N);
+					}
+				}
+			}
+		}
+	}
+
 	// Apply dynamic notch filter from FFT
 	if (_dynamic_notch_fft_available) {
 		for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) {
@@ -826,6 +1000,7 @@ void VehicleAngularVelocity::Run()
 
 	UpdateDynamicNotchEscRpm(time_now_us);
 	UpdateDynamicNotchFFT(time_now_us);
+	UpdateDynamicNotchRotorRpm(time_now_us);
 
 	if (_fifo_available) {
 		// process all outstanding fifo messages
@@ -975,6 +1150,10 @@ void VehicleAngularVelocity::PrintStatus()
 
 	perf_print_counter(_dynamic_notch_filter_fft_disable_perf);
 	perf_print_counter(_dynamic_notch_filter_fft_update_perf);
+
+	perf_print_counter(_dynamic_notch_filter_rotor_rpm_disable_perf);
+	perf_print_counter(_dynamic_notch_filter_rotor_rpm_init_perf);
+	perf_print_counter(_dynamic_notch_filter_rotor_rpm_update_perf);
 #endif // CONSTRAINED_FLASH
 }
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -47,6 +47,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
@@ -56,6 +57,7 @@
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/rpm.h>
 
 using namespace time_literals;
 
@@ -83,13 +85,15 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte
 
 	void DisableDynamicNotchEscRpm();
 	void DisableDynamicNotchFFT();
+	void DisableDynamicNotchRotorRpm();
 	void ParametersUpdate(bool force = false);
 
 	void ResetFilters(const hrt_abstime &time_now_us);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false);
 	void UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force = false);
 	void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false);
+	void UpdateDynamicNotchRotorRpm(const hrt_abstime &time_now_us, bool force = false);
 	bool UpdateSampleRate();
 
 	// scaled appropriately for current sensor
@@ -105,6 +109,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Subscription _esc_status_sub {ORB_ID(esc_status)};
 	uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
+	uORB::SubscriptionMultiArray<rpm_s> _rpm_sub{ORB_ID::rpm};
 #endif // !CONSTRAINED_FLASH
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -136,8 +141,10 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte
 #if !defined(CONSTRAINED_FLASH)
 
 	enum DynamicNotch {
+		// Power of two for bitwise comparison
 		EscRpm = 1,
 		FFT    = 2,
+		RotorRpm = 4,
 	};
 
 	static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s;
@@ -166,6 +173,21 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte
 	perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
 
 	bool _dynamic_notch_fft_available{false};
+
+	// Rotor RPM (dedicated tachometer, eg autorotating gyrocopter main rotor)
+	static constexpr int MAX_NUM_ROTOR_RPM_SENSORS = ORB_MULTI_MAX_INSTANCES;
+
+	using NotchFilterRotorRpmHarmonic = math::NotchFilter<float>[3][MAX_NUM_ROTOR_RPM_SENSORS];
+	NotchFilterRotorRpmHarmonic *_dynamic_notch_filter_rotor_rpm{nullptr};
+
+	int _rotor_rpm_harmonics{0};
+	px4::Bitset<MAX_NUM_ROTOR_RPM_SENSORS> _rotor_rpm_available{};
+	hrt_abstime _last_rotor_rpm_notch_update[MAX_NUM_ROTOR_RPM_SENSORS] {};
+
+	perf_counter_t _dynamic_notch_filter_rotor_rpm_disable_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_rotor_rpm_init_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_rotor_rpm_update_perf{nullptr};
+
 #endif // !CONSTRAINED_FLASH
 
 	// angular acceleration filter

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml

@@ -138,9 +138,10 @@ parameters:
       bit:
         0: ESC RPM
         1: FFT
+        2: Rotor RPM
       default: 0
       min: 0
-      max: 3
+      max: 7
     IMU_GYRO_DNF_BW:
       description:
         short: IMU gyro ESC notch filter bandwidth