dynamic-notch: general cleanup

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -652,13 +652,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchIceRpm(const hrt_abstime &time_no
 		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 < ORB_MULTI_MAX_INSTANCES; i++) {
+		for (unsigned i = 0; i < MAX_NUM_ICE_ENGINES; i++) {
 			internal_combustion_engine_status_s ice_status{};
 
 			if (_ice_status_sub[i].copy(&ice_status) &&
 			    (time_now_us < ice_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
 
-				const bool engine_running = (ice_status.state == internal_combustion_engine_status_s::STATE_RUNNING);
+				const bool engine_running = (ice_status.state == internal_combustion_engine_status_s::STATE_RUNNING
+							     || ice_status.state == internal_combustion_engine_status_s::STATE_STARTING);
 
 				if (engine_running && ice_status.engine_speed_rpm > 0.f) {
 					const float engine_hz = fabsf(ice_status.engine_speed_rpm) / 60.f;
@@ -695,7 +696,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchIceRpm(const hrt_abstime &time_no
 		}
 
 		// timeout handling
-		for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		for (unsigned i = 0; i < MAX_NUM_ICE_ENGINES; i++) {
 			if (_ice_available[i] && (time_now_us > _last_ice_rpm_notch_update[i] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
 				bool all_disabled = true;
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -109,6 +109,7 @@ private:
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 #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<internal_combustion_engine_status_s> _ice_status_sub{ORB_ID::internal_combustion_engine_status};
 
 	uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
@@ -144,8 +145,8 @@ private:
 
 	enum DynamicNotch {
 		EscRpm = 1,
-		FFT    = 2,
+		IceRpm = 2,
-		IceRpm = 4,
+		FFT    = 4,
 	};
 
 	static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s;
@@ -156,25 +157,24 @@ private:
 	using NotchFilterHarmonic = math::NotchFilter<float>[3][MAX_NUM_ESCS];
 	NotchFilterHarmonic *_dynamic_notch_filter_esc_rpm{nullptr};
 
-	// Internal Combustion Engine (ICE) dynamic notch filters (separate storage)
-	static constexpr int MAX_NUM_ICE_ENGINES = 5;
-	using NotchFilterIceHarmonic = math::NotchFilter<float>[3][MAX_NUM_ICE_ENGINES];
-	NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr};
-
 	int _esc_rpm_harmonics{0};
-
-	int _ice_rpm_harmonics{0};
 	px4::Bitset<MAX_NUM_ESCS> _esc_available{};
 	hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {};
 
-	// ICE availability/timeouts (separate from ESC)
-	px4::Bitset<MAX_NUM_ICE_ENGINES> _ice_available{};
-	hrt_abstime _last_ice_rpm_notch_update[MAX_NUM_ICE_ENGINES] {};
-
 	perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_esc_rpm_init_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
 
+	// ICE RPM
+	static constexpr int MAX_NUM_ICE_ENGINES = ORB_MULTI_MAX_INSTANCES;
+
+	using NotchFilterIceHarmonic = math::NotchFilter<float>[3][MAX_NUM_ICE_ENGINES];
+	NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr};
+
+	int _ice_rpm_harmonics{0};
+	px4::Bitset<MAX_NUM_ICE_ENGINES> _ice_available{};
+	hrt_abstime _last_ice_rpm_notch_update[MAX_NUM_ICE_ENGINES] {};
+
 	perf_counter_t _dynamic_notch_filter_ice_rpm_disable_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_ice_rpm_init_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_ice_rpm_update_perf{nullptr};

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -175,13 +175,13 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 20.0f);
 * IMU gyro dynamic notch filtering
 *
 * Enable bank of dynamically updating notch filters.
-* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
+* Requires RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
 * @group Sensors
 * @min 0
 * @max 7
 * @bit 0 ESC RPM
-* @bit 1 FFT
+* @bit 1 ICE RPM
-* @bit 2 ICE RPM
+* @bit 2 FFT
 */
 PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);