From 3c98c7119ebe956d1dc9c10379e9158567d16642 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 22 Dec 2014 13:47:20 -0700 Subject: use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation --- src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src/lib') 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); } -- cgit v1.2.3