diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-13 11:06:49 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-13 11:06:49 +0400 |
commit | 88149311ea687b62ba28e036e7de09ed2763f2bc (patch) | |
tree | 9c332da18da9580602e6040bb4eefc160a0c6ec5 /src/lib | |
parent | dfd9601b571057e73668d9b39d584bc4eb9cc305 (diff) | |
parent | 0b97dd2b776ce61fd53776f036230ea0089e26e9 (diff) | |
download | px4-firmware-88149311ea687b62ba28e036e7de09ed2763f2bc.tar.gz px4-firmware-88149311ea687b62ba28e036e7de09ed2763f2bc.tar.bz2 px4-firmware-88149311ea687b62ba28e036e7de09ed2763f2bc.zip |
Merge branch 'rc_timeout' into mpc_rc
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 7 | ||||
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 20 |
2 files changed, 22 insertions, 5 deletions
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce..6f640c9f9 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4..74cd5d78c 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; |