alt_est.c File Reference

Vertical Kalman filter (see alt_est.h). More...

#include "alt_est.h"
#include <math.h>
#include <string.h>

Include dependency graph for alt_est.c:

Go to the source code of this file.

Macros
#define	G0   9.80665f
#define	BARO_INIT_N   25u /* 0.5 s of samples to set the reference pressure */
#define	TOF_GATE_S   3.0f /* innovation gate, sigmas */
#define	TOF_REANCHOR   15u /* consecutive rejects (0.5 s at 30 Hz) */

Functions
void	alt_est_default_cfg (alt_est_cfg_t *c)
void	alt_est_init (alt_est_t *e, const alt_est_cfg_t *c)
float	alt_est_accel_up (const float q[4], const float f[3], float nom_g)
	vertical acceleration (m/s^2, + up, gravity removed) from the attitude and the specific force in body axes (g units, 1 g at rest = nom_g)
void	alt_est_predict (alt_est_t *e, float a, float dt)
static int	update (alt_est_t *e, float h0, float h2, float z, float r, float gate)
void	alt_est_baro (alt_est_t *e, float pa)
void	alt_est_tof (alt_est_t *e, float range, uint8_t ok, float ct)
	range_m: raw ToF range; status_ok: range status valid; cos_tilt: R33
void	alt_est_zero (alt_est_t *e)

Detailed Description

Vertical Kalman filter (see alt_est.h).

Definition in file alt_est.c.

Macro Definition Documentation

G0

#define G0   9.80665f

Definition at line 9 of file alt_est.c.

BARO_INIT_N

#define BARO_INIT_N   25u /* 0.5 s of samples to set the reference pressure */

Definition at line 10 of file alt_est.c.

TOF_GATE_S

#define TOF_GATE_S   3.0f /* innovation gate, sigmas */

Definition at line 11 of file alt_est.c.

TOF_REANCHOR

#define TOF_REANCHOR   15u /* consecutive rejects (0.5 s at 30 Hz) */

Definition at line 12 of file alt_est.c.

Function Documentation

alt_est_default_cfg()

void alt_est_default_cfg

(

alt_est_cfg_t *

c

)

Definition at line 14 of file alt_est.c.

References alt_est_cfg_t::sigma_acc, alt_est_cfg_t::sigma_acc_bias, alt_est_cfg_t::sigma_baro, alt_est_cfg_t::sigma_baro_bias, alt_est_cfg_t::sigma_tof, alt_est_cfg_t::tof_max_m, alt_est_cfg_t::tof_max_tilt_cos, alt_est_cfg_t::tof_min_m, and alt_est_cfg_t::tof_offset_m.

Referenced by ImuTask().

Here is the caller graph for this function:

alt_est_init()

void alt_est_init	(	alt_est_t *	e,
		const alt_est_cfg_t *	c
	)

Definition at line 27 of file alt_est.c.

References alt_est_t::c, alt_est_t::P, and alt_est_t::tof_age_s.

Referenced by ImuTask().

Here is the caller graph for this function:

alt_est_accel_up()

float alt_est_accel_up	(	const float	q[4],
		const float	f[3],
		float	nom_g
	)

vertical acceleration (m/s^2, + up, gravity removed) from the attitude and the specific force in body axes (g units, 1 g at rest = nom_g)

Definition at line 35 of file alt_est.c.

References G0, and n.

Referenced by on_state().

Here is the caller graph for this function:

alt_est_predict()

void alt_est_predict	(	alt_est_t *	e,
		float	a,
		float	dt
	)

Definition at line 47 of file alt_est.c.

References alt_est_t::c, F, G, N, alt_est_t::P, s, alt_est_cfg_t::sigma_acc, alt_est_cfg_t::sigma_acc_bias, alt_est_cfg_t::sigma_baro_bias, alt_est_t::tof_age_s, and alt_est_t::x.

Referenced by on_state().

Here is the caller graph for this function:

update()

static int update ( alt_est_t *  e, float  h0, float  h2, float  z, float  r, float  gate  )

static

Definition at line 83 of file alt_est.c.

References alt_est_t::P, and alt_est_t::x.

Referenced by alt_est_baro(), and alt_est_tof().

Here is the caller graph for this function:

alt_est_baro()

void alt_est_baro	(	alt_est_t *	e,
		float	pa
	)

Definition at line 102 of file alt_est.c.

References alt_est_t::baro_alt, BARO_INIT_N, alt_est_t::baro_ok, alt_est_t::c, alt_est_t::p_n, alt_est_t::p_ref, alt_est_t::p_sum, alt_est_t::pressure, alt_est_cfg_t::sigma_baro, update(), alt_est_t::valid, and alt_est_t::x.

Referenced by correct_events().

Here is the call graph for this function:

Here is the caller graph for this function:

alt_est_tof()

void alt_est_tof	(	alt_est_t *	e,
		float	range,
		uint8_t	ok,
		float	ct
	)

range_m: raw ToF range; status_ok: range status valid; cos_tilt: R33

Definition at line 120 of file alt_est.c.

References alt_est_t::c, alt_est_t::ground, alt_est_t::have_tof, s, alt_est_cfg_t::sigma_tof, alt_est_t::tof_age_s, TOF_GATE_S, alt_est_t::tof_ground, alt_est_t::tof_h, alt_est_cfg_t::tof_max_m, alt_est_cfg_t::tof_max_tilt_cos, alt_est_cfg_t::tof_min_m, alt_est_cfg_t::tof_offset_m, TOF_REANCHOR, alt_est_t::tof_rejects, alt_est_t::tof_valid, update(), alt_est_t::valid, and alt_est_t::x.

Referenced by take_ext().

Here is the call graph for this function:

Here is the caller graph for this function:

alt_est_zero()

void alt_est_zero

(

alt_est_t *

e

)

Definition at line 159 of file alt_est.c.

References alt_est_t::ground, and alt_est_t::x.