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

mathlib: Biquad filters use internal fields for parameter update

This commit is contained in:
Daniel Agar 2021-05-31 10:34:45 -04:00
parent 9661eddef8
commit 438b0abc7a
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -72,7 +72,7 @@ public:
_cutoff_freq = math::constrain(cutoff_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit _cutoff_freq = math::constrain(cutoff_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
_sample_freq = sample_freq; _sample_freq = sample_freq;
const float fr = sample_freq / _cutoff_freq; const float fr = _sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr); const float ohm = tanf(M_PI_F / fr);
const float c = 1.f + 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm; const float c = 1.f + 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm;

4
src/lib/mathlib/math/filter/NotchFilter.hpp
View File

@ -201,8 +201,8 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
_bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2); _bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2);
_sample_freq = sample_freq; _sample_freq = sample_freq;
const float alpha = tanf(M_PI_F * bandwidth / sample_freq); const float alpha = tanf(M_PI_F * _bandwidth / _sample_freq);
const float beta = -cosf(2.f * M_PI_F * notch_freq / sample_freq); const float beta = -cosf(2.f * M_PI_F * _notch_freq / _sample_freq);
const float a0_inv = 1.f / (alpha + 1.f); const float a0_inv = 1.f / (alpha + 1.f);
_b0 = a0_inv; _b0 = a0_inv;