Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: make sure the center frequency can never be zero

This commit is contained in:
Andy Piper 2020-04-14 09:51:58 +01:00 committed by Andrew Tridgell
parent dd987c9a19
commit 8407648316
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -111,13 +111,13 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, fl
} }
_sample_freq_hz = sample_freq_hz; _sample_freq_hz = sample_freq_hz;
// Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2
_notch_spread = bandwidth_hz / (32 * center_freq_hz);
const float nyquist_limit = sample_freq_hz * 0.48f; const float nyquist_limit = sample_freq_hz * 0.48f;
const float bandwidth_limit = bandwidth_hz * 0.52f; const float bandwidth_limit = bandwidth_hz * 0.52f;
// adjust the fundamental center frequency to be in the allowable range // adjust the fundamental center frequency to be in the allowable range
center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit); center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit);
// Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2
_notch_spread = bandwidth_hz / (32 * center_freq_hz);
if (_double_notch) { if (_double_notch) {
// position the individual notches so that the attenuation is no worse than a single notch // position the individual notches so that the attenuation is no worse than a single notch