Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 1 commit into from
Oct 9, 2023
Merged

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

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 4 additions & 9 deletions libraries/Filter/LowPassFilter2p.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Copy link
Member Author

@IamPete1 IamPete1 Oct 3, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

if (!is_positive(ret.cutoff_freq)) {
// zero cutoff means pass-thru
return;
}

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;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &params) {
_delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2));
initialised = true;
}

}

T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
Expand All @@ -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();
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No need to zero these setting initialised false will force a reset in the next apply call

initialised = false;
}

Expand All @@ -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);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using exactly 0,5 still resulted in numerical issues. 0.45 would probably work.

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;

Expand Down Expand Up @@ -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;
}
Comment on lines -106 to -109
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't need this check as we now check in the main apply function.

return _filter.apply(sample, _params);
}

Expand Down