diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 49081c29e45bd..f4de7b9f05f5d 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) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 82ae789ba5d97..5a35ea5b7416d 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[] = { @@ -177,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; @@ -192,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; @@ -200,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; } @@ -217,7 +224,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 @@ -261,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 @@ -278,7 +286,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) { @@ -311,10 +319,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++) { + for (uint16_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; } @@ -328,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 7de4df799df4e..898857b82e58e 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? diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 14ce778efa4cf..fa2c024721be9 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); diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index dd6f319f784ff..3011f09867c7a 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);