diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-21 14:41:03 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-21 14:41:03 +0100 |
commit | dcdde8ea88bf57b0432d2b64ed3ce606795c8d00 (patch) | |
tree | 0bf6e997ffe7f7a9cda98c832dbce39601a9242f /src/lib/mathlib | |
parent | 3a38b0fe359296aa19ec43ab82743aebeadb335c (diff) | |
parent | e8e4a3b5da058bd2ab8575c095dd74a5484333be (diff) | |
download | px4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.tar.gz px4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.tar.bz2 px4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.zip |
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge
Conflicts:
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
src/modules/uORB/topics/vehicle_attitude.h
Diffstat (limited to 'src/lib/mathlib')
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 5 |
1 files changed, 4 insertions, 1 deletions
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 6f640c9f9..c709a2834 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample) // no filtering return 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)) { @@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample) } float LowPassFilter2p::reset(float sample) { - _delay_element_1 = _delay_element_2 = sample; + float dval = sample / (_b0 + _b1 + _b2); + _delay_element_1 = dval; + _delay_element_2 = dval; return apply(sample); } |