36 #include <px4_platform_common/defines.h> 64 const float ohm = tanf(
M_PI_F / fr);
65 const float c = 1.0f + 2.0f * cosf(
M_PI_F / 4.0
f) * ohm + ohm * ohm;
71 _a1 = 2.0f * (ohm * ohm - 1.0f) / c;
72 _a2 = (1.0f - 2.0f * cosf(
M_PI_F / 4.0
f) * ohm + ohm * ohm) / c;
80 if (!PX4_ISFINITE(delay_element_0)) {
82 delay_element_0 = sample;
96 const float dval = sample / (
_b0 +
_b1 +
_b2);
98 if (PX4_ISFINITE(dval)) {
107 return apply(sample);
void set_cutoff_frequency(float sample_freq, float cutoff_freq)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
float reset(float sample)
float apply(float sample)
Add a new raw value to the filter.