dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

LowPassFilter: allow for filtering to be disabled 

using bandwidth of 0 gives no filtering
This commit is contained in:
Andrew Tridgell 2013-12-11 14:14:03 +11:00 committed by Lorenz Meier
parent 17502cbde4
commit e808e015dd
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -46,6 +46,10 @@ namespace math
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{ {
_cutoff_freq = cutoff_freq; _cutoff_freq = cutoff_freq;
if (_cutoff_freq <= 0.0f) {
// no filtering
return;
}
float fr = sample_freq/_cutoff_freq; float fr = sample_freq/_cutoff_freq;
float ohm = tanf(M_PI_F/fr); float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
float LowPassFilter2p::apply(float sample) float LowPassFilter2p::apply(float sample)
{ {
if (_cutoff_freq <= 0.0f) {
// no filtering
return sample;
}
// do the filtering // do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) { if (isnan(delay_element_0) || isinf(delay_element_0)) {