Filter: LowPassFilter2p: constrain cuttoff to 40% of sample rate #25175
Conversation
| @@ -17,13 +17,12 @@ DigitalBiquadFilter<T>::DigitalBiquadFilter() { | |||
|
|
|||
| template <class T> | |||
| T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params ¶ms) { | |||
| if(is_zero(params.cutoff_freq) || is_zero(params.sample_freq)) { | |||
| if(!is_positive(params.cutoff_freq) || !is_positive(params.sample_freq)) { | |||
There was a problem hiding this comment.
This changes to !is_positive to keep the same check as we do here:
ardupilot/libraries/Filter/LowPassFilter2p.cpp
Lines 54 to 57 in f72bfcc
For example if a user was to set the cutoff to -1 for disable the compute_params function would not update so you would be left with the configutation for the old cutoff and the filter would still work fine because -1 is not zero.
| return sample; | ||
| } | ||
|
|
||
| if (!initialised) { | ||
| reset(sample, params); | ||
| initialised = true; |
There was a problem hiding this comment.
Don't need to set initialised because the reset call does it for us.
ardupilot/libraries/Filter/LowPassFilter2p.cpp
Lines 45 to 48 in f72bfcc
| @@ -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(); | |||
There was a problem hiding this comment.
No need to zero these setting initialised false will force a reset in the next apply call
| @@ -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); | |||
There was a problem hiding this comment.
Using exactly 0,5 still resulted in numerical issues. 0.45 would probably work.
| if (!is_positive(_params.cutoff_freq)) { | ||
| // zero cutoff means pass-thru | ||
| return sample; | ||
| } |
There was a problem hiding this comment.
Don't need this check as we now check in the main apply function.
Currently its possible to get stuck in a boot loop if the frequency is set too high. For example set
INS_GYRO_FILTERto 1500 you will find the board will no longer boot all the way. The main goal of this PR is to fix that boot loop but I have also done a couple of other tidy ups.The stops numerical errors by constraining the cutoff down, we could also just turn off the filter as we would for a cutoff of zero.