ahrs_float_cmpl.h File Reference

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

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "subsystems/gps.h"

Include dependency graph for ahrs_float_cmpl.h:

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

Go to the source code of this file.

Data Structures
struct	AhrsFloatCmpl

Enumerations
enum	AhrsFCStatus { AHRS_FC_UNINIT, AHRS_FC_RUNNING }

Functions
void	ahrs_fc_init (void)
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)
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.h.

---

Data Structure Documentation

struct AhrsFloatCmpl

Definition at line 43 of file ahrs_float_cmpl.h.

Collaboration diagram for AhrsFloatCmpl:

Data Fields
uint16_t	accel_cnt	number of propagations since last accel update
float	accel_omega	filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
float	accel_zeta	filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
struct OrientationReps	body_to_imu
bool	correct_gravity	enable gravity correction during coordinated turns
uint8_t	gravity_heuristic_factor	sets how strongly the gravity heuristic reduces accel correction. Set to zero in order to disable gravity heuristic.
struct FloatRates	gyro_bias
bool	heading_aligned
struct FloatRates	imu_rate
bool	is_aligned
struct OrientationReps	ltp_to_body
struct FloatQuat	ltp_to_imu_quat
struct FloatRMat	ltp_to_imu_rmat
float	ltp_vel_norm	velocity norm for gravity correction during coordinated turns
bool	ltp_vel_norm_valid
uint16_t	mag_cnt	number of propagations since last mag update
struct FloatVect3	mag_h
float	mag_omega	filter cut-off frequency for correcting the attitude (heading) from magnetometer
float	mag_zeta	filter damping for correcting the gyro bias from magnetometer
struct FloatRates	rate_correction
enum AhrsFCStatus	status
float	weight

Enumeration Type Documentation

enum AhrsFCStatus

Enumerator
AHRS_FC_UNINIT
AHRS_FC_RUNNING

Definition at line 38 of file ahrs_float_cmpl.h.

Function Documentation

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

Definition at line 118 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_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, and AhrsFloatCmpl::status.

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 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_to_imu, AhrsFloatCmpl::correct_gravity, 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, 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 FloatRates *	gyro,
		float	dt
	)

Definition at line 145 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, ahrs_fc, alpha, 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_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 473 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, 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:

void ahrs_fc_recompute_ltp_to_body

(

void

)

Definition at line 525 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(), compute_body_orientation_and_rates(), and send_euler_int().

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 509 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 514 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 FloatVect3 *	accel,
		float	dt
	)

Definition at line 181 of file ahrs_float_cmpl.c.

References AhrsFloatCmpl::accel_cnt, AhrsFloatCmpl::accel_omega, AhrsFloatCmpl::accel_zeta, 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 398 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:

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 429 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 FloatVect3 *	mag,
		float	dt
	)

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

Variable Documentation

struct AhrsFloatCmpl ahrs_fc

Definition at line 80 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(), mag_cb(), send_bias(), send_euler(), send_euler_int(), send_filter_status(), and send_geo_mag().