fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 05:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation

Browse Source
This commit is contained in:
Mark Whitehorn
2015-01-13 09:06:00 -07:00
parent 78b603bb35
commit 3c98c7119e
3 changed files with 57 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+4 -1
View File
@@ -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);
}