diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp index 2c60cd916b..9fee044749 100644 --- a/src/lib/mathlib/math/filter/NotchFilter.hpp +++ b/src/lib/mathlib/math/filter/NotchFilter.hpp @@ -57,7 +57,7 @@ public: NotchFilter() = default; ~NotchFilter() = default; - void setParameters(float sample_freq, float notch_freq, float bandwidth); + bool setParameters(float sample_freq, float notch_freq, float bandwidth); /** * Add a new raw value to the filter using the Direct Form I @@ -217,13 +217,13 @@ protected: * conserving the filter's history */ template -void NotchFilter::setParameters(float sample_freq, float notch_freq, float bandwidth) +bool NotchFilter::setParameters(float sample_freq, float notch_freq, float bandwidth) { if ((sample_freq <= 0.f) || (notch_freq <= 0.f) || (bandwidth <= 0.f) || (notch_freq >= sample_freq / 2) || !isFinite(sample_freq) || !isFinite(notch_freq) || !isFinite(bandwidth)) { disable(); - return; + return false; } const float freq_min = sample_freq * 0.001f; @@ -231,11 +231,16 @@ void NotchFilter::setParameters(float sample_freq, float notch_freq, float ba const float notch_freq_new = math::max(notch_freq, freq_min); const float bandwidth_new = math::max(bandwidth, freq_min); - bool sample_freq_change = (fabsf(sample_freq - _sample_freq) > FLT_EPSILON); - bool bandwidth_change = (fabsf(bandwidth_new - _bandwidth) > FLT_EPSILON); + const float sample_freq_diff = fabsf(sample_freq - _sample_freq); + const float bandwidth_diff = fabsf(bandwidth_new - _bandwidth); + + const bool sample_freq_change = (sample_freq_diff > FLT_EPSILON); + const bool bandwidth_change = (bandwidth_diff > FLT_EPSILON); if (!sample_freq_change && !bandwidth_change) { - if (fabsf(notch_freq_new - _notch_freq) > FLT_EPSILON) { + const float notch_freq_diff = fabsf(notch_freq_new - _notch_freq); + + if (notch_freq_diff > FLT_EPSILON) { // only notch frequency has changed _notch_freq = notch_freq_new; @@ -244,15 +249,21 @@ void NotchFilter::setParameters(float sample_freq, float notch_freq, float ba _b1 = 2.f * beta * _b0; _a1 = _b1; - if (!isFinite(_b1)) { - disable(); + if (notch_freq_diff > _bandwidth) { + // force reset + _initialized = false; } - return; + if (!isFinite(_b1)) { + disable(); + return false; + } + + return true; } else { // no change, do nothing - return; + return true; } } @@ -273,7 +284,18 @@ void NotchFilter::setParameters(float sample_freq, float notch_freq, float ba if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a2)) { disable(); + return false; } + + // force reset if bandwidth or sample frequency changed by more than 1% + if (bandwidth_diff > 0.01f * _bandwidth) { + _initialized = false; + + } else if (sample_freq_diff > 0.01f * _sample_freq) { + _initialized = false; + } + + return true; } } // namespace math diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 46d01562a5..3226fbb686 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -58,11 +58,9 @@ VehicleAngularVelocity::~VehicleAngularVelocity() #if !defined(CONSTRAINED_FLASH) perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_free(_dynamic_notch_filter_esc_rpm_reset_perf); perf_free(_dynamic_notch_filter_esc_rpm_update_perf); perf_free(_dynamic_notch_filter_fft_disable_perf); - perf_free(_dynamic_notch_filter_fft_reset_perf); perf_free(_dynamic_notch_filter_fft_update_perf); #endif // CONSTRAINED_FLASH } @@ -392,7 +390,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) 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"); - _dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset"); _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM update"); } @@ -404,7 +401,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) 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_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset"); _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); } @@ -530,11 +526,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) // update filter parameters if frequency changed or forced if (reset || (notch_freq_diff > 0.1f)) { - // force reset if the notch frequency jumps significantly - if (notch_freq_diff > _param_imu_gyro_dnf_bw.get()) { - reset = true; - } - for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { const float frequency_hz = esc_hz * (harmonic + 1); @@ -547,16 +538,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) perf_count(_dynamic_notch_filter_esc_rpm_update_perf); } - if (reset) { - for (int axis = 0; axis < 3; axis++) { - for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { - _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(); - } - } - - perf_count(_dynamic_notch_filter_esc_rpm_reset_perf); - } - _dynamic_notch_esc_rpm_available = true; _esc_available.set(esc, true); _last_esc_rpm_notch_update[esc] = esc_report.timestamp; @@ -570,7 +551,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) for (int axis = 0; axis < 3; axis++) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { - _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0); + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].disable(); } } } @@ -629,18 +610,12 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) perf_count(_dynamic_notch_filter_fft_update_perf); } - // force reset if the notch frequency jumps significantly - if (force || reset || (notch_freq_diff > bandwidth)) { - nf.reset(); - perf_count(_dynamic_notch_filter_fft_reset_perf); - } - _dynamic_notch_fft_available = true; } else { // disable this notch filter (if it isn't already) if (force || !reset) { - nf.setParameters(0, 0, 0); + nf.disable(); perf_count(_dynamic_notch_filter_fft_disable_perf); } } @@ -885,11 +860,9 @@ void VehicleAngularVelocity::PrintStatus() perf_print_counter(_selection_changed_perf); #if !defined(CONSTRAINED_FLASH) perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_print_counter(_dynamic_notch_filter_esc_rpm_reset_perf); perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf); perf_print_counter(_dynamic_notch_filter_fft_disable_perf); - perf_print_counter(_dynamic_notch_filter_fft_reset_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 70fc59e539..00d122d836 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -156,11 +156,9 @@ private: 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_reset_perf{nullptr}; perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr}; perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr}; - perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr}; perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; bool _dynamic_notch_esc_rpm_available{false};