diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 9a51f0c4c0..9bf70e04b1 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -159,6 +159,37 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmo } } +/* + expand the number of filters at runtime, allowing for RPM sources such as lua scripts + */ +template +void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) +{ + uint8_t num_filters = _num_harmonics * num_notches * _composite_notches; + if (num_filters <= _num_filters) { + // enough already + return; + } + if (_alloc_has_failed) { + // we've failed to allocate before, don't try again + return; + } + /* + note that we rely on the semaphore in + AP_InertialSensor_Backend.cpp to make this thread safe + */ + auto filters = new NotchFilter[num_filters]; + if (filters == nullptr) { + _alloc_has_failed = true; + return; + } + memcpy(filters, _filters, sizeof(filters[0])*_num_filters); + auto _old_filters = _filters; + _filters = filters; + _num_filters = num_filters; + delete[] _old_filters; +} + /* update the underlying filters' center frequency using the current attenuation and quality this function is cheaper than init() because A & Q do not need to be recalculated @@ -216,6 +247,11 @@ void HarmonicNotchFilter::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; + if (num_centers > _num_filters) { + // alloc realloc of filters + expand_filter_count(num_centers); + } + _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index ac9d9c9265..6188a192d3 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -31,6 +31,8 @@ public: ~HarmonicNotchFilter(); // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches); + // expand filter bank with new filters + void expand_filter_count(uint8_t num_notches); // initialize the underlying filters using the provided filter parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); // update the underlying filters' center frequencies using center_freq_hz as the fundamental @@ -64,6 +66,9 @@ private: // number of enabled filters uint8_t _num_enabled_filters; bool _initialised; + + // have we failed to expand filters? + bool _alloc_has_failed; }; // Harmonic notch update mode