From 508da6bb0750d763f90f0e2cc4f674d2a1184006 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Mon, 13 Oct 2025 17:47:11 +0200 Subject: [PATCH 1/6] [wip] [not-tested] dynamic-notch: support ICE RPM input for notch filters --- .../VehicleAngularVelocity.cpp | 179 ++++++++++++++++++ .../VehicleAngularVelocity.hpp | 23 +++ 2 files changed, 202 insertions(+) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 23111d2f3fd6..52a058f96b2e 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); @@ -195,6 +199,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; @@ -492,6 +497,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"); @@ -573,6 +628,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) @@ -746,6 +907,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--) { @@ -825,6 +999,7 @@ void VehicleAngularVelocity::Run() } UpdateDynamicNotchEscRpm(time_now_us); + UpdateDynamicNotchIceRpm(time_now_us); UpdateDynamicNotchFFT(time_now_us); if (_fifo_available) { @@ -973,6 +1148,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 8aae8a3c03e9..8809604d9822 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 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte enum DynamicNotch { EscRpm = 1, FFT = 2, + IceRpm = 4, }; static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; @@ -148,14 +156,29 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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]); From a603055a9dba10edd180a659663f2c2dc5d19ca9 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Wed, 15 Oct 2025 16:24:47 +0200 Subject: [PATCH 2/6] dynamic-notch: general cleanup --- .../VehicleAngularVelocity.cpp | 7 ++--- .../VehicleAngularVelocity.hpp | 26 +++++++++---------- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 52a058f96b2e..8e382cb0d64e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -659,13 +659,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; @@ -702,7 +703,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; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8809604d9822..877c9fdf6566 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -109,6 +109,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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 _ice_status_sub{ORB_ID::internal_combustion_engine_status}; uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; @@ -144,8 +145,8 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte enum DynamicNotch { EscRpm = 1, - FFT = 2, - IceRpm = 4, + IceRpm = 2, + FFT = 4, }; static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; @@ -156,25 +157,24 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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}; + // ICE RPM + static constexpr int MAX_NUM_ICE_ENGINES = ORB_MULTI_MAX_INSTANCES; + + using NotchFilterIceHarmonic = math::NotchFilter[3][MAX_NUM_ICE_ENGINES]; + NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr}; + + int _ice_rpm_harmonics{0}; + px4::Bitset _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}; From 9e4d7cc2afd321cd6f1c02ff6e4eb8bf03daf9bf Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Wed, 15 Oct 2025 16:43:38 +0200 Subject: [PATCH 3/6] fix an oopsie --- .../sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 877c9fdf6566..6952b38d4b5f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -111,8 +111,6 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; 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 uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; From 1d07e1b26c93ac252812d0bbafa4c64c44bc05ff Mon Sep 17 00:00:00 2001 From: Antonio Moreira Date: Fri, 10 Jul 2026 15:07:04 +0200 Subject: [PATCH 4/6] feat(dynamic notch): receive feedback from Rpm.msg for DNF Signed-off-by: Antonio Moreira --- .../VehicleAngularVelocity.cpp | 299 +++++++++--------- .../VehicleAngularVelocity.hpp | 45 +-- .../imu_gyro_parameters.yaml | 3 +- 3 files changed, 174 insertions(+), 173 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 8e382cb0d64e..eb8b1138cd80 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -59,13 +59,15 @@ 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); + + 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); @@ -199,7 +201,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); + UpdateDynamicNotchRotorRpm(time_now_us, true); UpdateDynamicNotchFFT(time_now_us, true); _angular_velocity_raw_prev = angular_velocity_uncalibrated; @@ -497,64 +499,64 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) DisableDynamicNotchEscRpm(); } - // ICE dynamic notches: use same harmonics parameter for now - if (_param_imu_gyro_dnf_en.get() & DynamicNotch::IceRpm) { + 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"); + _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); + } - const int32_t ice_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); + } else { + DisableDynamicNotchFFT(); + } - 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; + // 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_ice_rpm == nullptr) { + if (_dynamic_notch_filter_rotor_rpm == nullptr) { - _dynamic_notch_filter_ice_rpm = new NotchFilterIceHarmonic[ice_rpm_harmonics]; + _dynamic_notch_filter_rotor_rpm = new NotchFilterRotorRpmHarmonic[rotor_rpm_harmonics]; - if (_dynamic_notch_filter_ice_rpm) { - _ice_rpm_harmonics = ice_rpm_harmonics; + if (_dynamic_notch_filter_rotor_rpm) { + _rotor_rpm_harmonics = rotor_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_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_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_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_ice_rpm_update_perf == nullptr) { - _dynamic_notch_filter_ice_rpm_update_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ICE update"); + 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 { - _ice_rpm_harmonics = 0; + _rotor_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); + 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_ice_rpm_disable_perf = nullptr; - _dynamic_notch_filter_ice_rpm_init_perf = nullptr; - _dynamic_notch_filter_ice_rpm_update_perf = nullptr; + _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 { - 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"); - _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); - } - - } else { - DisableDynamicNotchFFT(); + DisableDynamicNotchRotorRpm(); } #endif // !CONSTRAINED_FLASH @@ -628,105 +630,17 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() #endif // !CONSTRAINED_FLASH } -void VehicleAngularVelocity::DisableDynamicNotchIceRpm() +void VehicleAngularVelocity::DisableDynamicNotchRotorRpm() { #if !defined(CONSTRAINED_FLASH) - if (_dynamic_notch_filter_ice_rpm) { - for (int harmonic = 0; harmonic < _ice_rpm_harmonics; harmonic++) { + if (_dynamic_notch_filter_rotor_rpm) { + for (int harmonic = 0; harmonic < _rotor_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 < 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 - || 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; - 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 < 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; - - 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); + 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); } } } @@ -891,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) @@ -908,13 +907,13 @@ 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 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); } } } @@ -1000,8 +999,8 @@ void VehicleAngularVelocity::Run() } UpdateDynamicNotchEscRpm(time_now_us); - UpdateDynamicNotchIceRpm(time_now_us); UpdateDynamicNotchFFT(time_now_us); + UpdateDynamicNotchRotorRpm(time_now_us); if (_fifo_available) { // process all outstanding fifo messages @@ -1149,12 +1148,12 @@ 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); + + 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 } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 6952b38d4b5f..a33b3c16d35c 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -46,8 +46,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -57,7 +57,7 @@ #include #include #include -#include +#include using namespace time_literals; @@ -84,17 +84,16 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte inline float FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N = 1); void DisableDynamicNotchEscRpm(); - void DisableDynamicNotchIceRpm(); 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 UpdateDynamicNotchIceRpm(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 @@ -110,7 +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 _ice_status_sub{ORB_ID::internal_combustion_engine_status}; + uORB::SubscriptionMultiArray _rpm_sub{ORB_ID::rpm}; #endif // !CONSTRAINED_FLASH uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -142,9 +141,10 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte #if !defined(CONSTRAINED_FLASH) enum DynamicNotch { + // Power of two for bitwise comparison EscRpm = 1, - IceRpm = 2, - FFT = 4, + FFT = 2, + RotorRpm = 4, }; static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; @@ -163,20 +163,6 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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[3][MAX_NUM_ICE_ENGINES]; - NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr}; - - int _ice_rpm_harmonics{0}; - px4::Bitset _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}; - // 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]); @@ -187,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[3][MAX_NUM_ROTOR_RPM_SENSORS]; + NotchFilterRotorRpmHarmonic *_dynamic_notch_filter_rotor_rpm{nullptr}; + + int _rotor_rpm_harmonics{0}; + px4::Bitset _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 diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml index 9b07202eb72f..68b1a086d250 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml +++ b/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 From f834df7dffb46974aa593e7641344ea1904fd461 Mon Sep 17 00:00:00 2001 From: Antonio Moreira Date: Fri, 10 Jul 2026 16:18:16 +0200 Subject: [PATCH 5/6] chore: fixed format Signed-off-by: Antonio Moreira --- .../sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index eb8b1138cd80..a10e85f5c65e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -829,7 +829,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchRotorRpm(const hrt_abstime &time_ 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)); + freq_min + (harmonic * 0.5f * bandwidth_hz)); for (int axis = 0; axis < 3; axis++) { auto &nf = _dynamic_notch_filter_rotor_rpm[harmonic][axis][i]; From 6ed1c23ffddd9a2bb2eb64b888c85aa10dbf50a5 Mon Sep 17 00:00:00 2001 From: Antonio Moreira Date: Fri, 10 Jul 2026 16:18:47 +0200 Subject: [PATCH 6/6] chore(rpm_simulator): updated rpm_simulator to publish several messages in loop Signed-off-by: Antonio Moreira --- .../rpm/rpm_simulator/rpm_simulator.cpp | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 4926d1ca9f6b..ce3793373745 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/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 + * Usage: rpm_simulator [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 */ #include #include +#include #include #include @@ -52,8 +57,8 @@ 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 "); + if (argc < 2) { + PX4_INFO("Usage: rpm_simulator [duration_s]"); PX4_INFO("Exit. Without publishing any message."); return 0; } @@ -61,16 +66,26 @@ int rpm_simulator_main(int argc, char *argv[]) rpm_s rpm{}; uORB::Publication 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; }