Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: allow zero centre frequency to passthrough and disable notch

Browse Source
This commit is contained in:
Andy Piper 2023-11-03 17:05:23 +00:00 committed by Andrew Tridgell
parent eb4e3fc36a
commit a61fc423f0
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -217,7 +217,7 @@ void HarmonicNotchFilter<T>::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<T>::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) {

2
libraries/Filter/NotchFilter.cpp
View File

@ -71,7 +71,7 @@ void NotchFilter<T>::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);