ahrs_float_cmpl.c File Reference

Complementary filter in float to estimate the attitude, heading and gyro bias. More...

#include "modules/ahrs/ahrs_float_cmpl.h"
#include "modules/ahrs/ahrs_float_utils.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"
#include "modules/gps/gps.h"

Include dependency graph for ahrs_float_cmpl.c:

Go to the source code of this file.

Macros
#define	AHRS_ACCEL_OMEGA   0.063
#define	AHRS_ACCEL_ZETA   0.9
#define	AHRS_MAG_OMEGA   0.04
#define	AHRS_MAG_ZETA   0.9
#define	AHRS_GRAVITY_HEURISTIC_FACTOR   30
	by default use the gravity heuristic to reduce gain More...
#define	FIR_FILTER_SIZE   8

Functions
void	ahrs_fc_update_mag_full (struct FloatVect3 *mag, float dt)
void	ahrs_fc_update_mag_2d (struct FloatVect3 *mag, float dt)
void	ahrs_fc_update_mag_2d_dumb (struct FloatVect3 *mag)
void	ahrs_fc_init (void)
bool	ahrs_fc_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void	ahrs_fc_propagate (struct FloatRates *gyro, float dt)
void	ahrs_fc_update_accel (struct FloatVect3 *accel, float dt)
void	ahrs_fc_update_mag (struct FloatVect3 *mag, float dt)
void	ahrs_fc_update_gps (struct GpsState *gps_s)
void	ahrs_fc_update_heading (float heading)
	Update yaw based on a heading measurement. More...
void	ahrs_fc_realign_heading (float heading)
	Hard reset yaw to a heading. More...

Variables
struct AhrsFloatCmpl	ahrs_fc

Detailed Description

Complementary filter in float to estimate the attitude, heading and gyro bias.

Propagation can be done in rotation matrix or quaternion representation.

Definition in file ahrs_float_cmpl.c.

Macro Definition Documentation

AHRS_ACCEL_OMEGA

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 57 of file ahrs_float_cmpl.c.

AHRS_ACCEL_ZETA

#define AHRS_ACCEL_ZETA   0.9

Definition at line 60 of file ahrs_float_cmpl.c.

AHRS_GRAVITY_HEURISTIC_FACTOR

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 72 of file ahrs_float_cmpl.c.

AHRS_MAG_OMEGA

#define AHRS_MAG_OMEGA   0.04

Definition at line 64 of file ahrs_float_cmpl.c.

AHRS_MAG_ZETA

#define AHRS_MAG_ZETA   0.9

Definition at line 67 of file ahrs_float_cmpl.c.

FIR_FILTER_SIZE

#define FIR_FILTER_SIZE   8

Function Documentation

ahrs_fc_align()

bool ahrs_fc_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel,
		struct FloatVect3 *	lp_mag
	)

Definition at line 115 of file ahrs_float_cmpl.c.

References ahrs_fc, AHRS_FC_RUNNING, ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), float_rmat_of_quat(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_body_quat, AhrsFloatCmpl::ltp_to_body_rmat, and AhrsFloatCmpl::status.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_init()

void ahrs_fc_init

(

void

)

Definition at line 82 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, AhrsFloatCmpl::accel_omega, AhrsFloatCmpl::accel_zeta, AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, ahrs_fc, AHRS_FC_UNINIT, AHRS_GRAVITY_HEURISTIC_FACTOR, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, AhrsFloatCmpl::body_rate, AhrsFloatCmpl::correct_gravity, float_quat_identity(), FLOAT_RATES_ZERO, float_rmat_identity(), AhrsFloatCmpl::gravity_heuristic_factor, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_body_quat, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, AhrsFloatCmpl::status, and VECT3_ASSIGN.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_propagate()

void ahrs_fc_propagate	(	struct FloatRates *	gyro,
		float	dt
	)

Definition at line 142 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, ahrs_fc, alpha, AhrsFloatCmpl::body_rate, float_quat_integrate(), float_quat_normalize(), float_quat_of_rmat(), FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, float_rmat_integrate_fi(), float_rmat_of_quat(), float_rmat_reorthogonalize(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body_quat, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::rate_correction, RATES_COPY, RATES_SUB, and RATES_SUM.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_realign_heading()

void ahrs_fc_realign_heading

(

float

heading

)

Hard reset yaw to a heading.

Doesn't affect the bias. Sets ahrs_fc.heading_aligned to TRUE.

Parameters

heading

Heading in body frame, radians (CW/north)

Definition at line 460 of file ahrs_float_cmpl.c.

References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), float_rmat_of_quat(), heading, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_to_body_quat, AhrsFloatCmpl::ltp_to_body_rmat, FloatQuat::qi, QUAT_COPY, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by ahrs_fc_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_accel()

void ahrs_fc_update_accel	(	struct FloatVect3 *	accel,
		float	dt
	)

Definition at line 178 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, AhrsFloatCmpl::accel_omega, AhrsFloatCmpl::accel_zeta, ahrs_fc, AhrsFloatCmpl::body_rate, c2, AhrsFloatCmpl::correct_gravity, FIR_FILTER_SIZE, float_vect3_norm(), AhrsFloatCmpl::gravity_heuristic_factor, AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::ltp_vel_norm, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsFloatCmpl::rate_correction, RATES_ADD_SCALED_VECT, RMAT_ELMT, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, VECT3_SMUL, and AhrsFloatCmpl::weight.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_gps()

void ahrs_fc_update_gps

(

struct GpsState *

gps_s

)

Definition at line 388 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_realign_heading(), ahrs_fc_update_heading(), course, GPS_FIX_3D, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_vel_norm, and AhrsFloatCmpl::ltp_vel_norm_valid.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_heading()

void ahrs_fc_update_heading

(

float

heading

)

Update yaw based on a heading measurement.

e.g. from GPS course

Parameters

heading

Heading in body frame, radians (CW/north)

Definition at line 419 of file ahrs_float_cmpl.c.

References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_rmat_vmult(), AhrsFloatCmpl::gyro_bias, heading, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::rate_correction, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatVect2::x, FloatVect2::y, and FloatVect3::z.

Referenced by ahrs_fc_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_mag()

void ahrs_fc_update_mag	(	struct FloatVect3 *	mag,
		float	dt
	)

Definition at line 269 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_full(), and AhrsFloatCmpl::mag_cnt.

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_mag_2d()

void ahrs_fc_update_mag_2d	(	struct FloatVect3 *	mag,
		float	dt
	)

Definition at line 316 of file ahrs_float_cmpl.c.

References ahrs_fc, float_rmat_transp_vmult(), float_rmat_vmult(), float_vect2_normalize(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, AhrsFloatCmpl::rate_correction, RATES_ADD_SCALED_VECT, VECT2_COPY, FloatVect2::x, FloatVect3::x, FloatVect2::y, and FloatVect3::y.

Referenced by ahrs_fc_update_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_update_mag_2d_dumb()

void ahrs_fc_update_mag_2d_dumb

(

struct FloatVect3 *

mag

)

Definition at line 361 of file ahrs_float_cmpl.c.

References ahrs_fc, float_eulers_of_rmat(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body_rmat, FloatEulers::phi, AhrsFloatCmpl::rate_correction, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

ahrs_fc_update_mag_full()

void ahrs_fc_update_mag_full	(	struct FloatVect3 *	mag,
		float	dt
	)

Definition at line 286 of file ahrs_float_cmpl.c.

References ahrs_fc, float_rmat_vmult(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, AhrsFloatCmpl::rate_correction, RATES_ADD_SCALED_VECT, and VECT3_CROSS_PRODUCT.

Referenced by ahrs_fc_update_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

ahrs_fc

struct AhrsFloatCmpl ahrs_fc

Definition at line 78 of file ahrs_float_cmpl.c.

Referenced by accel_cb(), ahrs_fc_align(), ahrs_fc_init(), ahrs_fc_propagate(), ahrs_fc_realign_heading(), ahrs_fc_update_accel(), ahrs_fc_update_gps(), ahrs_fc_update_heading(), ahrs_fc_update_mag(), ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_2d_dumb(), ahrs_fc_update_mag_full(), aligner_cb(), compute_body_orientation_and_rates(), geo_mag_cb(), gyro_cb(), mag_cb(), send_bias(), send_euler(), send_euler_int(), send_filter_status(), and send_geo_mag().