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

ArduPilot · Oct 9, 2023 · d8e18a0 · d8e18a0
1 parent 48224da
commit d8e18a0
Showing 1 changed file with 4 additions and 9 deletions.
diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp
@@ -17,13 +17,12 @@ DigitalBiquadFilter<T>::DigitalBiquadFilter() {
 
 template <class T>
 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;
     }
 
     if (!initialised) {
         reset(sample, params);
-        initialised = true;
     }
 
     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>
 void DigitalBiquadFilter<T>::reset() { 
-    _delay_element_1 = _delay_element_2 = T();
     initialised = false;
 }
 
@@ -49,14 +47,15 @@ void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &p
 
 template <class T>
 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;
     if (!is_positive(ret.cutoff_freq)) {
         // zero cutoff means pass-thru
         return;
     }
 
-    float fr = sample_freq/cutoff_freq;
+    float fr = ret.sample_freq / ret.cutoff_freq;
     float ohm = tanf(M_PI/fr);
     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>
 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);
 }