From 3beafd526ee4e65acda28c043f6ced9d4c4ee9cf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 24 Oct 2023 13:25:53 +0100 Subject: [PATCH 1/4] AP_InertialSensor: init all notch center frequencies --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 49081c29e45bd0..f4de7b9f05f5dd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -986,7 +986,9 @@ AP_InertialSensor::init(uint16_t loop_rate) if (!notch.params.enabled() && !fft_enabled) { continue; } - notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); + for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) { + notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz(); + } notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) From 8b9798726ad7365fabb108bcecfb4454139a6200 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 3 Nov 2023 17:05:23 +0000 Subject: [PATCH 2/4] Filter: allow zero centre frequency to passthrough and disable notch --- libraries/Filter/HarmonicNotchFilter.cpp | 4 ++-- libraries/Filter/NotchFilter.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 82ae789ba5d97a..3182d4b1647a71 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -217,7 +217,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) // adjust the fundamental center frequency to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit); + center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q @@ -278,7 +278,7 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 1.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); if (_composite_notches != 2) { // only enable the filter if its center frequency is below the nyquist frequency if (notch_center < nyquist_limit) { diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 14ce778efa4cf2..fa2c024721be91 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -71,7 +71,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A); From dc21a645c8f0f5df066920d7d3dd0230844f8b2c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Nov 2023 17:19:58 +1100 Subject: [PATCH 3/4] Filter: added optional notch debug logging this has helped find multiple bugs --- libraries/Filter/HarmonicNotchFilter.cpp | 31 ++++++++++++++++++++++++ libraries/Filter/NotchFilter.h | 4 +++ 2 files changed, 35 insertions(+) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 3182d4b1647a71..b2d1ad37c419b4 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,9 +20,21 @@ #include "HarmonicNotchFilter.h" #include +#include +#include +#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters +/* + optional logging for SITL only of all notch frequencies + */ +#ifndef NOTCH_DEBUG_LOGGING +#define NOTCH_DEBUG_LOGGING 0 +#endif + + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -311,10 +323,29 @@ T HarmonicNotchFilter::apply(const T &sample) return sample; } +#if NOTCH_DEBUG_LOGGING + static int dfd = -1; + if (dfd == -1) { + dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644); + } +#endif + T output = sample; for (uint8_t i = 0; i < _num_enabled_filters; i++) { +#if NOTCH_DEBUG_LOGGING + if (!_filters[i].initialised) { + ::dprintf(dfd, "------- "); + } else { + ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz); + } +#endif output = _filters[i].apply(output); } +#if NOTCH_DEBUG_LOGGING + if (_num_enabled_filters > 0) { + ::dprintf(dfd, "\n"); + } +#endif return output; } diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index dd6f319f784fff..3011f09867c7a4 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -26,9 +26,13 @@ #include +template +class HarmonicNotchFilter; + template class NotchFilter { public: + friend class HarmonicNotchFilter; // set parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); From dc5373a66bd543d496deddc1f277c6ee749b6ccd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Nov 2023 17:29:55 +1100 Subject: [PATCH 4/4] Filter: fixed uint8_t bug in total filters and expand_filter_count bug this gets the right number of notches on quadplanes, but is still very bad in fwd flight --- libraries/Filter/HarmonicNotchFilter.cpp | 22 +++++++++------------- libraries/Filter/HarmonicNotchFilter.h | 6 +++--- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index b2d1ad37c419b4..5a35ea5b7416d2 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -189,13 +189,8 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harm 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) +void HarmonicNotchFilter::expand_filter_count(uint16_t total_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; @@ -204,7 +199,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) note that we rely on the semaphore in AP_InertialSensor_Backend.cpp to make this thread safe */ - auto filters = new NotchFilter[num_filters]; + auto filters = new NotchFilter[total_notches]; if (filters == nullptr) { _alloc_has_failed = true; return; @@ -212,7 +207,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) memcpy(filters, _filters, sizeof(filters[0])*_num_filters); auto _old_filters = _filters; _filters = filters; - _num_filters = num_filters; + _num_filters = total_notches; delete[] _old_filters; } @@ -273,15 +268,16 @@ 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) { + uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + if (total_notches > _num_filters) { // alloc realloc of filters - expand_filter_count(num_centers); + expand_filter_count(total_notches); } _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q - for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { const uint8_t harmonic_n = i / num_centers; const uint8_t center_n = i % num_centers; // the filters are ordered by center and then harmonic so @@ -331,7 +327,7 @@ T HarmonicNotchFilter::apply(const T &sample) #endif T output = sample; - for (uint8_t i = 0; i < _num_enabled_filters; i++) { + for (uint16_t i = 0; i < _num_enabled_filters; i++) { #if NOTCH_DEBUG_LOGGING if (!_filters[i].initialised) { ::dprintf(dfd, "------- "); @@ -359,7 +355,7 @@ void HarmonicNotchFilter::reset() return; } - for (uint8_t i = 0; i < _num_filters; i++) { + for (uint16_t i = 0; i < _num_filters; i++) { _filters[i].reset(); } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 7de4df799df4e7..898857b82e58ec 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -32,7 +32,7 @@ class HarmonicNotchFilter { // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches); // expand filter bank with new filters - void expand_filter_count(uint8_t num_notches); + void expand_filter_count(uint16_t total_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 @@ -60,11 +60,11 @@ class HarmonicNotchFilter { // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters - uint8_t _num_filters; + uint16_t _num_filters; // pre-calculated number of harmonics uint8_t _num_harmonics; // number of enabled filters - uint8_t _num_enabled_filters; + uint16_t _num_enabled_filters; bool _initialised; // have we failed to expand filters?