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

Filter: LowPassFilter2p: constrain cuttoff to 40% of sample rate

This commit is contained in:
Iampete1 2023-10-03 19:02:50 +01:00 committed by Randy Mackay
parent 48224dad92
commit d8e18a0fc7
1 changed files with 4 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/Filter/LowPassFilter2p.cpp
View File

@ -17,13 +17,12 @@ DigitalBiquadFilter<T>::DigitalBiquadFilter() {
template <class T> template <class T>
T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &params) { T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &params) {
if(is_zero(params.cutoff_freq) || is_zero(params.sample_freq)) { if(!is_positive(params.cutoff_freq) || !is_positive(params.sample_freq)) {
return sample; return sample;
} }
if (!initialised) { if (!initialised) {
reset(sample, params); reset(sample, params);
initialised = true;
} }
T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2; T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
@ -37,7 +36,6 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
template <class T> template <class T>
void DigitalBiquadFilter<T>::reset() { void DigitalBiquadFilter<T>::reset() {
_delay_element_1 = _delay_element_2 = T();
initialised = false; initialised = false;
} }
@ -49,14 +47,15 @@ void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &p
template <class T> template <class T>
void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret) { void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret) {
ret.cutoff_freq = cutoff_freq; // Keep well under Nyquist limit
ret.cutoff_freq = MIN(cutoff_freq, sample_freq * 0.4);
ret.sample_freq = sample_freq; ret.sample_freq = sample_freq;
if (!is_positive(ret.cutoff_freq)) { if (!is_positive(ret.cutoff_freq)) {
// zero cutoff means pass-thru // zero cutoff means pass-thru
return; return;
} }
float fr = sample_freq/cutoff_freq; float fr = ret.sample_freq / ret.cutoff_freq;
float ohm = tanf(M_PI/fr); float ohm = tanf(M_PI/fr);
float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm; float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm;
@ -103,10 +102,6 @@ float LowPassFilter2p<T>::get_sample_freq(void) const {
template <class T> template <class T>
T LowPassFilter2p<T>::apply(const T &sample) { T LowPassFilter2p<T>::apply(const T &sample) {
if (!is_positive(_params.cutoff_freq)) {
// zero cutoff means pass-thru
return sample;
}
return _filter.apply(sample, _params); return _filter.apply(sample, _params);
} }