ahrs_float_cmpl.c File Reference

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

#include "subsystems/ahrs/ahrs_float_cmpl.h"
#include "subsystems/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 "subsystems/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 Int32Vect3 *mag, float dt)
void	ahrs_fc_update_mag_2d (struct Int32Vect3 *mag, float dt)
void	ahrs_fc_update_mag_2d_dumb (struct Int32Vect3 *mag)
void	ahrs_fc_init (void)
bool_t	ahrs_fc_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ahrs_fc_propagate (struct Int32Rates *gyro, float dt)
void	ahrs_fc_update_accel (struct Int32Vect3 *accel, float dt)
void	ahrs_fc_update_mag (struct Int32Vect3 *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...
void	ahrs_fc_set_body_to_imu (struct OrientationReps *body_to_imu)
void	ahrs_fc_set_body_to_imu_quat (struct FloatQuat *q_b2i)
void	ahrs_fc_recompute_ltp_to_body (void)

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

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 61 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_ACCEL_ZETA   0.9

Definition at line 64 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 76 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_MAG_OMEGA   0.04

Definition at line 68 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_MAG_ZETA   0.9

Definition at line 71 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define FIR_FILTER_SIZE   8

Referenced by ahrs_fc_update_accel().

Function Documentation

bool_t ahrs_fc_align	(	struct Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	lp_mag
	)

Definition at line 122 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(), FALSE, float_rmat_of_quat(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, RATES_COPY, RATES_FLOAT_OF_BFP, AhrsFloatCmpl::status, and TRUE.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_fc_init

(

void

)

Definition at line 86 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_to_imu, AhrsFloatCmpl::correct_gravity, FALSE, float_quat_identity(), FLOAT_RATES_ZERO, AhrsFloatCmpl::gravity_heuristic_factor, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::imu_rate, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, orientationSetIdentity(), AhrsFloatCmpl::status, TRUE, and VECT3_ASSIGN.

Referenced by ahrs_fc_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_fc_propagate	(	struct Int32Rates *	gyro,
		float	dt
	)

Definition at line 151 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, ahrs_fc, 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::imu_rate, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::rate_correction, RATES_COPY, RATES_FLOAT_OF_BFP, 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:

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 479 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_recompute_ltp_to_body(), AhrsFloatCmpl::body_to_imu, FLOAT_ANGLE_NORMALIZE, float_quat_comp(), float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), float_rmat_of_quat(), AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetQuat_f(), FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and TRUE.

Referenced by ahrs_fc_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_fc_recompute_ltp_to_body

(

void

)

Definition at line 531 of file ahrs_float_cmpl.c.

References ahrs_fc, AhrsFloatCmpl::body_to_imu, float_quat_comp_inv(), AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_quat, orientationGetQuat_f(), and orientationSetQuat_f().

Referenced by ahrs_fc_realign_heading(), ahrs_fc_update_heading(), and compute_body_orientation_and_rates().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_fc_set_body_to_imu

(

struct OrientationReps *

body_to_imu

)

Definition at line 515 of file ahrs_float_cmpl.c.

References ahrs_fc_set_body_to_imu_quat(), and orientationGetQuat_f().

Here is the call graph for this function:

void ahrs_fc_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 520 of file ahrs_float_cmpl.c.

References ahrs_fc, AhrsFloatCmpl::body_to_imu, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetQuat_f(), orientationGetRMat_f(), and orientationSetQuat_f().

Referenced by ahrs_fc_set_body_to_imu(), and body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_fc_update_accel	(	struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 189 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, AhrsFloatCmpl::accel_omega, AhrsFloatCmpl::accel_zeta, ACCELS_FLOAT_OF_BFP, ahrs_fc, AhrsFloatCmpl::body_to_imu, AhrsFloatCmpl::correct_gravity, FIR_FILTER_SIZE, float_rmat_transp_ratemult(), float_rmat_vmult(), float_vect3_norm(), AhrsFloatCmpl::gravity_heuristic_factor, AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::imu_rate, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::ltp_vel_norm, AhrsFloatCmpl::ltp_vel_norm_valid, orientationGetRMat_f(), 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:

void ahrs_fc_update_gps

(

struct GpsState *

gps_s

)

Definition at line 404 of file ahrs_float_cmpl.c.

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

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

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 435 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_recompute_ltp_to_body(), FLOAT_ANGLE_NORMALIZE, float_rmat_vmult(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetRMat_f(), 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:

void ahrs_fc_update_mag	(	struct Int32Vect3 *	mag,
		float	dt
	)

Definition at line 281 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:

void ahrs_fc_update_mag_2d	(	struct Int32Vect3 *	mag,
		float	dt
	)

Definition at line 330 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_imu_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, MAGS_FLOAT_OF_BFP, 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:

void ahrs_fc_update_mag_2d_dumb

(

struct Int32Vect3 *

mag

)

Definition at line 376 of file ahrs_float_cmpl.c.

References ahrs_fc, float_eulers_of_rmat(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_imu_rmat, MAGS_FLOAT_OF_BFP, 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:

void ahrs_fc_update_mag_full	(	struct Int32Vect3 *	mag,
		float	dt
	)

Definition at line 298 of file ahrs_float_cmpl.c.

References ahrs_fc, float_rmat_vmult(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::mag_cnt, AhrsFloatCmpl::mag_h, AhrsFloatCmpl::mag_omega, AhrsFloatCmpl::mag_zeta, MAGS_FLOAT_OF_BFP, 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

struct AhrsFloatCmpl ahrs_fc

Definition at line 84 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_recompute_ltp_to_body(), ahrs_fc_set_body_to_imu_quat(), 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(), and mag_cb().