From ae75ea94eb038d491f06bd050f5d04e63e61a463 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 6 Nov 2023 16:51:02 +1100 Subject: [PATCH] Filter: fixed filter expansion for other than motors based RPM sources this also fixes the uint8_t storage of a number than can be greater than 256. Max total notches in a single HarmonicNotchFilter is currently 12*16*3 for 12 ESCs, with INS_HNTCH_HMNCS=0xFFFF and triple notch --- libraries/Filter/HarmonicNotchFilter.cpp | 10 +++++++--- libraries/Filter/HarmonicNotchFilter.h | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 8da5d3bdc9..d2fcdc6d8d 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -204,6 +204,10 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harm template void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { + if (total_notches <= _num_filters) { + // enough already + return; + } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -228,7 +232,7 @@ void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) set the center frequency of a single notch */ template -void HarmonicNotchFilter::set_center_frequency(uint8_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) +void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) { const float nyquist_limit = _sample_freq_hz * 0.48f; auto ¬ch = _filters[idx]; @@ -307,7 +311,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q - for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { if ((1U<::update(uint8_t num_centers, const float center_freq // adjust the frequencies to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; if (total_notches > _num_filters) { // alloc realloc of filters expand_filter_count(total_notches); diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index e74313a444..1961387ade 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -47,7 +47,7 @@ public: spread_mul is a scale factor for spreading of double or triple notch harmonic_mul is the multiplier for harmonics, 1 is for the fundamental */ - void set_center_frequency(uint8_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); + void set_center_frequency(uint16_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); // apply a sample to each of the underlying filters in turn T apply(const T &sample);