alt_est.h File Reference

Vertical estimator: height, climb rate, baro bias, accelerometer bias. More...

#include <stdint.h>

Include dependency graph for alt_est.h:

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

Go to the source code of this file.

Data Structures
struct	alt_est_cfg_t
struct	alt_est_t

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)
void	alt_est_predict (alt_est_t *e, float a_up, float dt)
void	alt_est_baro (alt_est_t *e, float pa)
void	alt_est_tof (alt_est_t *e, float range_m, uint8_t status_ok, float cos_tilt)
	range_m: raw ToF range; status_ok: range status valid; cos_tilt: R33
void	alt_est_zero (alt_est_t *e)
float	alt_est_accel_up (const float q[4], const float f_body_g[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)
static float	alt_est_h (const alt_est_t *e)
static float	alt_est_vz (const alt_est_t *e)

Detailed Description

Vertical estimator: height, climb rate, baro bias, accelerometer bias.

x = [ h v b_baro b_acc ] h m above the reference (+ up) v m/s (+ up) predict : vertical acceleration from the IEKF attitude and the accelerometer (gravity removed), 240 Hz baro : z = h + b_baro (altitude from pressure, 50 Hz) ToF : z = h - ground (range x cos(tilt) - lens offset, 30 Hz)

Without ToF the baro bias is not observable: it is frozen (no process noise), h then follows the baro. With ToF (ACwing Flow) the baro bias is tracked, the height is the ToF height above the floor, and the baro carries the estimate through ToF gaps (out of range, > 30 deg tilt). A ToF reading that jumps (step onto a table, an object under the drone) is gated; if it keeps disagreeing for 0.5 s the ground level is re-anchored. Below the ToF minimum range (~40 mm: landed, the lens is 11 mm above the floor) the reading means "on the ground" (tof_ground).

alt_est_zero() sets the reference (FLIGHT.altRel = 0 at the take-off point). Host-testable: no HAL, no RTOS.

Definition in file alt_est.h.

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_predict()

void alt_est_predict	(	alt_est_t *	e,
		float	a_up,
		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:

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_m,
		uint8_t	status_ok,
		float	cos_tilt
	)

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.

alt_est_accel_up()

float alt_est_accel_up	(	const float	q[4],
		const float	f_body_g[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_h()

static float alt_est_h ( const alt_est_t *  e )

inlinestatic

Definition at line 73 of file alt_est.h.

References alt_est_t::x.

Referenced by flight_service(), and publish_est().

Here is the caller graph for this function:

alt_est_vz()

static float alt_est_vz ( const alt_est_t *  e )

inlinestatic

Definition at line 74 of file alt_est.h.

References alt_est_t::x.

Referenced by flight_service(), and publish_est().

Here is the caller graph for this function: