From cdf8cd274c22b351d24ed928c0ecd10f9171a9ba Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Mon, 13 Oct 2025 17:47:11 +0200 Subject: [PATCH] [wip] [not-tested] dynamic-notch: support ICE RPM input for notch filters --- .../VehicleAngularVelocity.cpp | 179 ++++++++++++++++++ .../VehicleAngularVelocity.hpp | 23 +++ .../imu_gyro_parameters.c | 3 +- 3 files changed, 204 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 01750b4259..d195f6fa72 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -59,9 +59,13 @@ VehicleAngularVelocity::~VehicleAngularVelocity() #if !defined(CONSTRAINED_FLASH) delete[] _dynamic_notch_filter_esc_rpm; + delete[] _dynamic_notch_filter_ice_rpm; perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); perf_free(_dynamic_notch_filter_esc_rpm_init_perf); perf_free(_dynamic_notch_filter_esc_rpm_update_perf); + perf_free(_dynamic_notch_filter_ice_rpm_disable_perf); + perf_free(_dynamic_notch_filter_ice_rpm_init_perf); + perf_free(_dynamic_notch_filter_ice_rpm_update_perf); perf_free(_dynamic_notch_filter_fft_disable_perf); perf_free(_dynamic_notch_filter_fft_update_perf); @@ -194,6 +198,7 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) // force reset notch filters on any scale change UpdateDynamicNotchEscRpm(time_now_us, true); + UpdateDynamicNotchIceRpm(time_now_us, true); UpdateDynamicNotchFFT(time_now_us, true); _angular_velocity_raw_prev = angular_velocity_uncalibrated; @@ -485,6 +490,56 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) DisableDynamicNotchEscRpm(); } + // ICE dynamic notches: use same harmonics parameter for now + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::IceRpm) { + + const int32_t ice_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); + + if (_dynamic_notch_filter_ice_rpm && (ice_rpm_harmonics != _ice_rpm_harmonics)) { + delete[] _dynamic_notch_filter_ice_rpm; + _dynamic_notch_filter_ice_rpm = nullptr; + _ice_rpm_harmonics = 0; + } + + if (_dynamic_notch_filter_ice_rpm == nullptr) { + + _dynamic_notch_filter_ice_rpm = new NotchFilterIceHarmonic[ice_rpm_harmonics]; + + if (_dynamic_notch_filter_ice_rpm) { + _ice_rpm_harmonics = ice_rpm_harmonics; + + if (_dynamic_notch_filter_ice_rpm_disable_perf == nullptr) { + _dynamic_notch_filter_ice_rpm_disable_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ICE disable"); + } + + if (_dynamic_notch_filter_ice_rpm_init_perf == nullptr) { + _dynamic_notch_filter_ice_rpm_init_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ICE init"); + } + + if (_dynamic_notch_filter_ice_rpm_update_perf == nullptr) { + _dynamic_notch_filter_ice_rpm_update_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ICE update"); + } + + } else { + _ice_rpm_harmonics = 0; + + perf_free(_dynamic_notch_filter_ice_rpm_disable_perf); + perf_free(_dynamic_notch_filter_ice_rpm_init_perf); + perf_free(_dynamic_notch_filter_ice_rpm_update_perf); + + _dynamic_notch_filter_ice_rpm_disable_perf = nullptr; + _dynamic_notch_filter_ice_rpm_init_perf = nullptr; + _dynamic_notch_filter_ice_rpm_update_perf = nullptr; + } + } + + } else { + DisableDynamicNotchIceRpm(); + } + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) { if (_dynamic_notch_filter_fft_disable_perf == nullptr) { _dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable"); @@ -566,6 +621,112 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() #endif // !CONSTRAINED_FLASH } +void VehicleAngularVelocity::DisableDynamicNotchIceRpm() +{ +#if !defined(CONSTRAINED_FLASH) + + if (_dynamic_notch_filter_ice_rpm) { + for (int harmonic = 0; harmonic < _ice_rpm_harmonics; harmonic++) { + for (int axis = 0; axis < 3; axis++) { + for (int engine = 0; engine < MAX_NUM_ICE_ENGINES; engine++) { + _dynamic_notch_filter_ice_rpm[harmonic][axis][engine].disable(); + _ice_available.set(engine, false); + perf_count(_dynamic_notch_filter_ice_rpm_disable_perf); + } + } + } + } + +#endif // !CONSTRAINED_FLASH +} + + +void VehicleAngularVelocity::UpdateDynamicNotchIceRpm(const hrt_abstime &time_now_us, bool force) +{ +#if !defined(CONSTRAINED_FLASH) + const bool enabled = _dynamic_notch_filter_ice_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::IceRpm); + + 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 < ORB_MULTI_MAX_INSTANCES; 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); + + if (engine_running && ice_status.engine_speed_rpm > 0.f) { + const float engine_hz = fabsf(ice_status.engine_speed_rpm) / 60.f; + const bool force_update = force || !_ice_available[i]; + + for (int harmonic = 0; harmonic < _ice_rpm_harmonics; harmonic++) { + const float frequency_hz = math::max(engine_hz * (harmonic + 1), + freq_min + (harmonic * 0.5f * bandwidth_hz)); + + for (int axis = 0; axis < 3; axis++) { + auto &nf = _dynamic_notch_filter_ice_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_ice_rpm_update_perf); + + if (!nf.initialized()) { + perf_count(_dynamic_notch_filter_ice_rpm_init_perf); + axis_init[axis] = true; + } + } + } + } + } + + _ice_available.set(i, true); + _last_ice_rpm_notch_update[i] = ice_status.timestamp; + } + } + } + + // timeout handling + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_ice_available[i] && (time_now_us > _last_ice_rpm_notch_update[i] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + bool all_disabled = true; + + for (int harmonic = _ice_rpm_harmonics - 1; harmonic >= 0; harmonic--) { + for (int axis = 0; axis < 3; axis++) { + auto &nf = _dynamic_notch_filter_ice_rpm[harmonic][axis][i]; + + if (nf.getNotchFreq() > 0.f) { + if (nf.initialized() && !axis_init[axis]) { + nf.disable(); + perf_count(_dynamic_notch_filter_ice_rpm_disable_perf); + axis_init[axis] = true; + } + } + + if (nf.getNotchFreq() > 0.f) { + all_disabled = false; + } + } + } + + if (all_disabled) { + _ice_available.set(i, false); + } + } + } + } + +#endif // !CONSTRAINED_FLASH +} + void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force) { #if !defined(CONSTRAINED_FLASH) @@ -739,6 +900,19 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int } } + // Apply dynamic notch filter from ICE RPM (separate filters) + if (_dynamic_notch_filter_ice_rpm) { + for (int inst = 0; inst < MAX_NUM_ESCS; inst++) { + if (_ice_available[inst]) { + for (int harmonic = 0; harmonic < _ice_rpm_harmonics; harmonic++) { + if (_dynamic_notch_filter_ice_rpm[harmonic][axis][inst].getNotchFreq() > 0.f) { + _dynamic_notch_filter_ice_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--) { @@ -818,6 +992,7 @@ void VehicleAngularVelocity::Run() } UpdateDynamicNotchEscRpm(time_now_us); + UpdateDynamicNotchIceRpm(time_now_us); UpdateDynamicNotchFFT(time_now_us); if (_fifo_available) { @@ -966,6 +1141,10 @@ void VehicleAngularVelocity::PrintStatus() perf_print_counter(_dynamic_notch_filter_esc_rpm_init_perf); perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf); + perf_print_counter(_dynamic_notch_filter_ice_rpm_disable_perf); + perf_print_counter(_dynamic_notch_filter_ice_rpm_init_perf); + perf_print_counter(_dynamic_notch_filter_ice_rpm_update_perf); + perf_print_counter(_dynamic_notch_filter_fft_disable_perf); perf_print_counter(_dynamic_notch_filter_fft_update_perf); #endif // CONSTRAINED_FLASH diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8aae8a3c03..8809604d98 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,7 @@ #include #include #include +#include using namespace time_literals; @@ -82,6 +84,7 @@ private: inline float FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N = 1); void DisableDynamicNotchEscRpm(); + void DisableDynamicNotchIceRpm(); void DisableDynamicNotchFFT(); void ParametersUpdate(bool force = false); @@ -89,6 +92,8 @@ private: 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 UpdateDynamicNotchIceRpm(const hrt_abstime &time_now_us, bool force = false); + void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false); bool UpdateSampleRate(); @@ -104,6 +109,8 @@ 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::SubscriptionMultiArray _ice_status_sub{ORB_ID::internal_combustion_engine_status}; + uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; #endif // !CONSTRAINED_FLASH @@ -138,6 +145,7 @@ private: enum DynamicNotch { EscRpm = 1, FFT = 2, + IceRpm = 4, }; static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; @@ -148,14 +156,29 @@ private: using NotchFilterHarmonic = math::NotchFilter[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[3][MAX_NUM_ICE_ENGINES]; + NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr}; + int _esc_rpm_harmonics{0}; + + int _ice_rpm_harmonics{0}; px4::Bitset _esc_available{}; hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {}; + // ICE availability/timeouts (separate from ESC) + px4::Bitset _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}; + 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}; + // 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]); diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index 2f53e0d779..82df797905 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -178,9 +178,10 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 20.0f); * Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN). * @group Sensors * @min 0 -* @max 3 +* @max 7 * @bit 0 ESC RPM * @bit 1 FFT +* @bit 2 ICE RPM */ PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);