Simple first order low pass filter with bilinear transform. More...

#include "std.h"
#include "math/pprz_algebra_int.h"

Include dependency graph for low_pass_filter.h:

This graph shows which files directly or indirectly include this file:

Data Structures
struct	FirstOrderLowPass
	First order low pass filter structure. More...

struct	SecondOrderLowPass
	Second order low pass filter structure. More...

struct	SecondOrderLowPass_int

struct	Butterworth4LowPass
	Fourth order Butterworth low pass filter. More...

struct	Butterworth4LowPass_int
	Fourth order Butterworth low pass filter(fixed point version). More...

Macros
#define	INT32_FILT_FRAC 8

Typedefs
typedef struct SecondOrderLowPass	Butterworth2LowPass
	Second order Butterworth low pass filter.

typedef struct SecondOrderLowPass_int	Butterworth2LowPass_int
	Second order Butterworth low pass filter(fixed point version).

Functions
static void	init_first_order_low_pass (struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
	Init first order low pass filter.

static float	update_first_order_low_pass (struct FirstOrderLowPass *filter, const float value)
	Update first order low pass filter state with a new value.

static float	reset_first_order_low_pass (struct FirstOrderLowPass *filter, const float value)
	Reset the first order low-pass filter to a specific value.

static float	get_first_order_low_pass (const struct FirstOrderLowPass *filter)
	Get current value of the first order low pass filter.

static void	update_first_order_low_pass_tau (struct FirstOrderLowPass *filter, const float tau, const float sample_time)
	Update time constant (tau parameter) for first order low pass filter.

static void	init_second_order_low_pass (struct SecondOrderLowPass *filter, const float tau, const float Q, const float sample_time, float value)
	Init second order low pass filter.

static float	reset_second_order_low_pass (struct SecondOrderLowPass *filter, const float value)
	Reset the second order low-pass filter to a specific value.

static float	update_second_order_low_pass (struct SecondOrderLowPass *filter, const float value)
	Update second order low pass filter state with a new value.

static float	get_second_order_low_pass (const struct SecondOrderLowPass *filter)
	Get current value of the second order low pass filter.

static void	init_second_order_low_pass_int (struct SecondOrderLowPass_int *filter, const float cut_off, const float Q, float sample_time, int32_t value)
	Init second order low pass filter(fixed point version).

static int32_t	update_second_order_low_pass_int (struct SecondOrderLowPass_int *filter, const int32_t value)
	Update second order low pass filter state with a new value(fixed point version).

static int32_t	get_second_order_low_pass_int (const struct SecondOrderLowPass_int *filter)
	Get current value of the second order low pass filter(fixed point version).

static void	init_butterworth_2_low_pass (Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
	Init a second order Butterworth filter.

static float	update_butterworth_2_low_pass (Butterworth2LowPass *filter, const float value)
	Update second order Butterworth low pass filter state with a new value.

static float	reset_butterworth_2_low_pass (Butterworth2LowPass *filter, const float value)
	Reset a Butterworth low-pass filter to a specific value.

static float	get_butterworth_2_low_pass (const Butterworth2LowPass *filter)
	Get current value of the second order Butterworth low pass filter.

static void	init_butterworth_2_low_pass_int (Butterworth2LowPass_int *filter, const float cut_off, const float sample_time, int32_t value)
	Init a second order Butterworth filter.

static int32_t	update_butterworth_2_low_pass_int (Butterworth2LowPass_int *filter, const int32_t value)
	Update second order Butterworth low pass filter state with a new value(fixed point version).

static int32_t	get_butterworth_2_low_pass_int (const Butterworth2LowPass_int *filter)
	Get current value of the second order Butterworth low pass filter(fixed point version).

static void	init_butterworth_4_low_pass (Butterworth4LowPass *filter, const float tau, const float sample_time, const float value)
	Init a fourth order Butterworth filter.

static float	update_butterworth_4_low_pass (Butterworth4LowPass *filter, const float value)
	Update fourth order Butterworth low pass filter state with a new value.

static float	get_butterworth_4_low_pass (const Butterworth4LowPass *filter)
	Get current value of the fourth order Butterworth low pass filter.

static void	reset_butterworth_4_low_pass (Butterworth4LowPass *filter, const float value)
	Reset a Butterworth low-pass filter to a specific value.

static void	init_butterworth_4_low_pass_int (Butterworth4LowPass_int *filter, const float cut_off, const float sample_time, int32_t value)
	Init a fourth order Butterworth filter(fixed point version).

static int32_t	update_butterworth_4_low_pass_int (Butterworth4LowPass_int *filter, const int32_t value)
	Update fourth order Butterworth low pass filter state with a new value(fixed point version).

static int32_t	get_butterworth_4_low_pass_int (const Butterworth4LowPass_int *filter)
	Get current value of the fourth order Butterworth low pass filter(fixed point version).

Detailed Description

Simple first order low pass filter with bilinear transform.

@FIXME: INT and FLOAT implementations are inconsistent, tau and cut_off freq are mixed. Suggestion: Change everything to freq.

Definition in file low_pass_filter.h.

Data Structure Documentation

◆ FirstOrderLowPass

struct FirstOrderLowPass

First order low pass filter structure.

using bilinear z transform

Definition at line 42 of file low_pass_filter.h.

Data Fields
float	last_in
float	last_out
float	time_const

◆ SecondOrderLowPass

struct SecondOrderLowPass

Second order low pass filter structure.

using biquad filter with bilinear z transform

http://en.wikipedia.org/wiki/Digital_biquad_filter http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform

Laplace continious form:

H(s) = ----------------— s^2/w^2 + s/w*Q + 1

Polynomial discrete form:

   b0 + b1 z^-1 + b2 z^-2

H(z) = -------------------— a0 + a1 z^-1 + a2 z^-2

with: a0 = 1 a1 = 2*(K^2 - 1) / (K^2 + K/Q + 1) a2 = (K^2 - K/Q + 1) / (K^2 + K/Q + 1) b0 = K^2 / (K^2 + K/Q + 1) b1 = 2*b0 b2 = b0 K = tan(pi*Fc/Fs) ~ pi*Fc/Fs = Ts/(2*tau) Fc: cutting frequency Fs: sampling frequency Ts: sampling period tau: time constant (tau = 1/(2*pi*Fc)) Q: gain at cutoff frequency

Note that b[0]=b[2], so we don't need to save b[2]

Definition at line 154 of file low_pass_filter.h.

Data Fields
float	a[2]	denominator gains
float	b[2]	numerator gains
float	i[2]	input history
float	o[2]	output history

◆ SecondOrderLowPass_int

struct SecondOrderLowPass_int

Definition at line 226 of file low_pass_filter.h.

Data Fields
int32_t	a[2]	denominator gains
int32_t	b[2]	numerator gains
int32_t	i[2]	input history
int32_t	loop_gain	loop gain
int32_t	o[2]	output history

◆ Butterworth4LowPass

struct Butterworth4LowPass

Fourth order Butterworth low pass filter.

using two cascaded second order filters

Definition at line 400 of file low_pass_filter.h.

Collaboration diagram for Butterworth4LowPass:

Data Fields
struct SecondOrderLowPass	lp1
struct SecondOrderLowPass	lp2

◆ Butterworth4LowPass_int

struct Butterworth4LowPass_int

Fourth order Butterworth low pass filter(fixed point version).

using two cascaded second order filters

Definition at line 465 of file low_pass_filter.h.

Collaboration diagram for Butterworth4LowPass_int:

Data Fields
struct SecondOrderLowPass_int	lp1
struct SecondOrderLowPass_int	lp2

Macro Definition Documentation

◆ INT32_FILT_FRAC

#define INT32_FILT_FRAC 8

Definition at line 36 of file low_pass_filter.h.

Typedef Documentation

◆ Butterworth2LowPass

typedef struct SecondOrderLowPass Butterworth2LowPass

Second order Butterworth low pass filter.

Definition at line 299 of file low_pass_filter.h.

◆ Butterworth2LowPass_int

typedef struct SecondOrderLowPass_int Butterworth2LowPass_int

Second order Butterworth low pass filter(fixed point version).

Definition at line 354 of file low_pass_filter.h.

Function Documentation

◆ get_butterworth_2_low_pass()

static float get_butterworth_2_low_pass ( const Butterworth2LowPass * filter )

inlinestatic

Get current value of the second order Butterworth low pass filter.

Parameters

filter second order Butterworth low pass filter structure

Returns: current value of the filter

Definition at line 347 of file low_pass_filter.h.

References get_second_order_low_pass().

Referenced by airspeed_ms45xx_i2c_change_tau(), get_butterworth_2_low_pass_array(), get_butterworth_2_low_pass_rates(), get_butterworth_2_low_pass_vect3(), and reinit_cf2().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_butterworth_2_low_pass_int()

static int32_t get_butterworth_2_low_pass_int ( const Butterworth2LowPass_int * filter )

inlinestatic

Get current value of the second order Butterworth low pass filter(fixed point version).

Parameters

filter second order Butterworth low pass filter structure

Returns: current value of the filter

Definition at line 391 of file low_pass_filter.h.

References SecondOrderLowPass_int::o.

Referenced by mavlink_send_highres_imu().

Here is the caller graph for this function:

◆ get_butterworth_4_low_pass()

static float get_butterworth_4_low_pass ( const Butterworth4LowPass * filter )

inlinestatic

Get current value of the fourth order Butterworth low pass filter.

Parameters

filter fourth order Butterworth low pass filter structure

Returns: current value of the filter

Definition at line 444 of file low_pass_filter.h.

References Butterworth4LowPass::lp2, and SecondOrderLowPass::o.

Referenced by get_butterworth_4_complementary(), get_butterworth_4_low_pass_array(), get_butterworth_4_low_pass_rates(), get_butterworth_4_low_pass_vect3(), and reinit_cf4().

Here is the caller graph for this function:

◆ get_butterworth_4_low_pass_int()

static int32_t get_butterworth_4_low_pass_int ( const Butterworth4LowPass_int * filter )

inlinestatic

Get current value of the fourth order Butterworth low pass filter(fixed point version).

Parameters

filter fourth order Butterworth low pass filter structure

Returns: current value of the filter

Definition at line 510 of file low_pass_filter.h.

References Butterworth4LowPass_int::lp2, and SecondOrderLowPass_int::o.

◆ get_first_order_low_pass()

static float get_first_order_low_pass ( const struct FirstOrderLowPass * filter )

inlinestatic

Get current value of the first order low pass filter.

Parameters

filter first order low pass filter structure

Returns: current value of the filter

Definition at line 101 of file low_pass_filter.h.

References FirstOrderLowPass::last_out.

Referenced by get_first_order_complementary(), get_first_order_low_pass_array(), get_first_order_low_pass_rates(), and get_first_order_low_pass_vect3().

Here is the caller graph for this function:

◆ get_second_order_low_pass()

static float get_second_order_low_pass ( const struct SecondOrderLowPass * filter )

inlinestatic

Get current value of the second order low pass filter.

Parameters

filter second order low pass filter structure

Returns: current value of the filter

Definition at line 221 of file low_pass_filter.h.

References SecondOrderLowPass::o.

Referenced by get_butterworth_2_low_pass(), and get_second_order_complementary().

Here is the caller graph for this function:

◆ get_second_order_low_pass_int()

static int32_t get_second_order_low_pass_int ( const struct SecondOrderLowPass_int * filter )

inlinestatic

Get current value of the second order low pass filter(fixed point version).

Parameters

filter second order low pass filter structure

Returns: current value of the filter

Definition at line 292 of file low_pass_filter.h.

References SecondOrderLowPass_int::o.

◆ init_butterworth_2_low_pass()

static void init_butterworth_2_low_pass	(	Butterworth2LowPass *	filter,
		const float	tau,
		const float	sample_time,
		const float	value
	)

inlinestatic

Init a second order Butterworth filter.

based on the generic second order filter with Q = 0.7071 = 1/sqrt(2)

http://en.wikipedia.org/wiki/Butterworth_filter

Parameters

filter	second order Butterworth low pass filter structure
tau	time constant of the second order low pass filter
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 313 of file low_pass_filter.h.

References foo, and init_second_order_low_pass().

Referenced by adaptive_notch_filter_init(), airspeed_consistency_init(), airspeed_ets_init(), airspeed_ms45xx_i2c_change_tau(), airspeed_uavcan_init(), aoa_t4_init_filters(), approach_moving_target_set_low_pass_freq(), eff_scheduling_rotwing_init(), ekf_aw_wrapper_init(), ground_detect_init(), guidance_indi_enter(), guidance_indi_init(), guidance_indi_quadplane_init(), imu_init(), indi_init_filters(), init_butterworth_2_low_pass_array(), init_butterworth_2_low_pass_vect3(), init_cf2(), init_filter(), init_filters(), ms45xx_i2c_init(), reinit_cf2(), sdp3x_init(), sonar_adc_init(), ssa_t4_init_filters(), and stabilization_indi_update_filt_freq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_butterworth_2_low_pass_int()

static void init_butterworth_2_low_pass_int	(	Butterworth2LowPass_int *	filter,
		const float	cut_off,
		const float	sample_time,
		int32_t	value
	)

inlinestatic

Init a second order Butterworth filter.

based on the generic second order filter with Q = 0.7071 = 1/sqrt(2)

http://en.wikipedia.org/wiki/Butterworth_filter

Parameters

filter	second order Butterworth low pass filter structure
cut_off	Cutoff frequency of the filter with -3dB
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 368 of file low_pass_filter.h.

References foo, and init_second_order_low_pass_int().

Referenced by hff_init(), jevois_mavlink_filter_init(), and stabilization_attitude_heli_indi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_butterworth_4_low_pass()

static void init_butterworth_4_low_pass	(	Butterworth4LowPass *	filter,
		const float	tau,
		const float	sample_time,
		const float	value
	)

inlinestatic

Init a fourth order Butterworth filter.

based on two generic second order filters with Q1 = 1.30651 and Q2 = 0.541184

http://en.wikipedia.org/wiki/Butterworth_filter

Parameters

filter	fourth order Butterworth low pass filter structure
tau	time constant of the fourth order low pass filter
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 418 of file low_pass_filter.h.

References foo, init_second_order_low_pass(), Butterworth4LowPass::lp1, and Butterworth4LowPass::lp2.

Referenced by init_butterworth_4_complementary(), init_butterworth_4_low_pass_array(), init_butterworth_4_low_pass_vect3(), init_cf4(), and reinit_cf4().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_butterworth_4_low_pass_int()

static void init_butterworth_4_low_pass_int	(	Butterworth4LowPass_int *	filter,
		const float	cut_off,
		const float	sample_time,
		int32_t	value
	)

inlinestatic

Init a fourth order Butterworth filter(fixed point version).

based on two generic second order filters with Q1 = 1.30651 and Q2 = 0.541184

http://en.wikipedia.org/wiki/Butterworth_filter

Parameters

filter	fourth order Butterworth low pass filter structure
cut_off	Cutoff frequency of the filter with -3dB
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 483 of file low_pass_filter.h.

References foo, init_second_order_low_pass_int(), Butterworth4LowPass_int::lp1, and Butterworth4LowPass_int::lp2.

Here is the call graph for this function:

◆ init_first_order_low_pass()

static void init_first_order_low_pass	(	struct FirstOrderLowPass *	filter,
		float	tau,
		const float	sample_time,
		float	value
	)

inlinestatic

Init first order low pass filter.

Laplace transform in continious time: 1 H(s) = ------— 1 + tau*s

Parameters

filter	first order low pass filter structure
tau	time constant of the first order low pass filter
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 60 of file low_pass_filter.h.

References foo, FirstOrderLowPass::last_in, FirstOrderLowPass::last_out, and FirstOrderLowPass::time_const.

Referenced by cloud_sensor_init(), indi_init_filters(), init_filters(), init_first_order_complementary(), init_first_order_low_pass_array(), init_first_order_low_pass_vect3(), nps_fdm_init(), rpm_sensor_init(), stabilization_indi_simple_reset_r_filter_cutoff(), stabilization_indi_update_filt_freq(), and sys_id_chirp_init().

Here is the caller graph for this function:

◆ init_second_order_low_pass()

static void init_second_order_low_pass	(	struct SecondOrderLowPass *	filter,
		const float	tau,
		const float	Q,
		const float	sample_time,
		float	value
	)

inlinestatic

Init second order low pass filter.

Parameters

filter	second order low pass filter structure
tau	time constant of the second order low pass filter
Q	Q value of the second order low pass filter
sample_time	sampling period of the signal
value	initial value of the filter

Definition at line 170 of file low_pass_filter.h.

References SecondOrderLowPass::a, SecondOrderLowPass::b, foo, SecondOrderLowPass::i, K, and SecondOrderLowPass::o.

Referenced by init_butterworth_2_low_pass(), init_butterworth_4_low_pass(), and init_second_order_complementary().

Here is the caller graph for this function:

◆ init_second_order_low_pass_int()

static void init_second_order_low_pass_int	(	struct SecondOrderLowPass_int *	filter,
		const float	cut_off,
		const float	Q,
		float	sample_time,
		int32_t	value
	)

inlinestatic

Init second order low pass filter(fixed point version).

Parameters

filter	second order low pass filter structure
cut_off	Cutoff frequency of the filter with -3dB
Q	Q value of the second order low pass filter
sample_time	sampling period of the signal
value	initial value of the filter