ahrs_int_cmpl_quat.h File Reference

Quaternion complementary filter (fixed-point). More...

#include "modules/ahrs/ahrs.h"
#include "modules/gps/gps.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_orientation_conversion.h"

Include dependency graph for ahrs_int_cmpl_quat.h:

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

Go to the source code of this file.

Data Structures
struct	AhrsIntCmplQuat
	Ahrs implementation specifc values. More...

Enumerations
enum	AhrsICQStatus { AHRS_ICQ_UNINIT , AHRS_ICQ_RUNNING }

Functions
void	ahrs_icq_init (void)
bool	ahrs_icq_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ahrs_icq_propagate (struct Int32Rates *gyro, float dt)
void	ahrs_icq_update_accel (struct Int32Vect3 *accel, float dt)
void	ahrs_icq_update_mag (struct Int32Vect3 *mag, float dt)
void	ahrs_icq_update_gps (struct GpsState *gps_s)
void	ahrs_icq_update_heading (int32_t heading)
	Update yaw based on a heading measurement. More...
void	ahrs_icq_realign_heading (int32_t heading)
	Hard reset yaw to a heading. More...
void	ahrs_icq_set_accel_gains (void)
	update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta More...
static void	ahrs_int_cmpl_quat_SetAccelOmega (float omega)
static void	ahrs_int_cmpl_quat_SetAccelZeta (float zeta)
void	ahrs_icq_set_mag_gains (void)
	update pre-computed kp and ki gains from mag_omega and mag_zeta More...
static void	ahrs_int_cmpl_quat_SetMagOmega (float omega)
static void	ahrs_int_cmpl_quat_SetMagZeta (float zeta)

Variables
struct AhrsIntCmplQuat	ahrs_icq
	Default Rate filter Low pass. More...

Detailed Description

Quaternion complementary filter (fixed-point).

Estimate the attitude, heading and gyro bias.

Definition in file ahrs_int_cmpl_quat.h.

---

Data Structure Documentation

AhrsIntCmplQuat

struct AhrsIntCmplQuat

Ahrs implementation specifc values.

Definition at line 48 of file ahrs_int_cmpl_quat.h.

Collaboration diagram for AhrsIntCmplQuat:

Data Fields
uint16_t	accel_cnt	number of propagations since last accel update
float	accel_inv_ki
float	accel_inv_kp
float	accel_omega	filter cut-off frequency for correcting the attitude from accels. (pseudo-gravity measurement) only update through ahrs_int_cmpl_quat_SetAccelOmega
float	accel_zeta	filter damping for correcting the gyro-bias from accels. (pseudo-gravity measurement) only update through ahrs_int_cmpl_quat_SetAccelZeta
struct Int32Rates	body_rate
bool	correct_gravity	enable gravity vector correction by removing centrifugal acceleration
uint8_t	gravity_heuristic_factor	sets how strongly the gravity heuristic reduces accel correction. Set to zero in order to disable gravity heuristic.
struct Int32Rates	gyro_bias
bool	heading_aligned
struct Int64Rates	high_rez_bias
struct Int64Quat	high_rez_quat
bool	is_aligned
struct Int32Quat	ltp_to_body_quat
int32_t	ltp_vel_norm
bool	ltp_vel_norm_valid
uint16_t	mag_cnt	number of propagations since last mag update
struct Int32Vect3	mag_h
float	mag_ki
float	mag_kp
float	mag_omega	filter cut-off frequency for correcting the attitude (heading) from magnetometer. only update through ahrs_int_cmpl_quat_SetMagOmega
float	mag_zeta	filter damping for correcting the gyro bias from magnetometer. only update through ahrs_int_cmpl_quat_SetMagZeta
struct Int32Rates	rate_correction
enum AhrsICQStatus	status	status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
float	weight

Enumeration Type Documentation

AhrsICQStatus

enum AhrsICQStatus

Enumerator
AHRS_ICQ_UNINIT
AHRS_ICQ_RUNNING

Definition at line 40 of file ahrs_int_cmpl_quat.h.

Function Documentation

ahrs_icq_align()

bool ahrs_icq_align	(	struct Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	lp_mag
	)

Definition at line 161 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AHRS_ICQ_RUNNING, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsIntCmplQuat::high_rez_bias, INT_RATES_LSHIFT, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_body_quat, RATES_COPY, and AhrsIntCmplQuat::status.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_init()

void ahrs_icq_init

(

void

)

Definition at line 119 of file ahrs_int_cmpl_quat.c.

References AhrsIntCmplQuat::accel_cnt, AhrsIntCmplQuat::accel_omega, AhrsIntCmplQuat::accel_zeta, AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, AHRS_GRAVITY_HEURISTIC_FACTOR, ahrs_icq, ahrs_icq_set_accel_gains(), ahrs_icq_set_mag_gains(), AHRS_ICQ_UNINIT, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, AhrsIntCmplQuat::body_rate, AhrsIntCmplQuat::correct_gravity, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsIntCmplQuat::high_rez_bias, int32_quat_identity(), INT_RATES_ZERO, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_body_quat, AhrsIntCmplQuat::ltp_vel_norm_valid, MAG_BFP_OF_REAL, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::mag_h, AhrsIntCmplQuat::mag_omega, AhrsIntCmplQuat::mag_zeta, AhrsIntCmplQuat::rate_correction, AhrsIntCmplQuat::status, and VECT3_ASSIGN.

Referenced by ahrs_icq_register(), and ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_propagate()

void ahrs_icq_propagate	(	struct Int32Rates *	gyro,
		float	dt
	)

Definition at line 190 of file ahrs_int_cmpl_quat.c.

References AhrsIntCmplQuat::accel_cnt, ahrs_icq, AhrsIntCmplQuat::body_rate, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_quat, int32_quat_integrate_fi(), int32_quat_normalize(), INT_RATES_ZERO, AhrsIntCmplQuat::ltp_to_body_quat, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::rate_correction, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_SDIV, and RATES_SMUL.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_realign_heading()

void ahrs_icq_realign_heading

(

int32_t

heading

)

Hard reset yaw to a heading.

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

Parameters

heading

Heading in body frame, radians (CW/north) with INT32_ANGLE_FRAC

Definition at line 618 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, heading, AhrsIntCmplQuat::heading_aligned, int32_quat_comp_norm_shortest(), int32_quat_inv_comp_norm_shortest(), int32_quat_normalize(), AhrsIntCmplQuat::ltp_to_body_quat, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, QUAT_COPY, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by ahrs_icq_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_set_accel_gains()

void ahrs_icq_set_accel_gains

(

void

)

update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta

Definition at line 223 of file ahrs_int_cmpl_quat.c.

References AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, AhrsIntCmplQuat::accel_omega, AhrsIntCmplQuat::accel_zeta, and ahrs_icq.

Referenced by ahrs_icq_init(), ahrs_int_cmpl_quat_SetAccelOmega(), and ahrs_int_cmpl_quat_SetAccelZeta().

Here is the caller graph for this function:

ahrs_icq_set_mag_gains()

void ahrs_icq_set_mag_gains

(

void

)

update pre-computed kp and ki gains from mag_omega and mag_zeta

Definition at line 397 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AhrsIntCmplQuat::mag_ki, AhrsIntCmplQuat::mag_kp, AhrsIntCmplQuat::mag_omega, and AhrsIntCmplQuat::mag_zeta.

Referenced by ahrs_icq_init(), and ahrs_int_cmpl_quat_SetMagOmega().

Here is the caller graph for this function:

ahrs_icq_update_accel()

void ahrs_icq_update_accel	(	struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 244 of file ahrs_int_cmpl_quat.c.

References ACC_FROM_CROSS_FRAC, AhrsIntCmplQuat::accel_cnt, AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, ACCELS_FLOAT_OF_BFP, ahrs_icq, AhrsIntCmplQuat::body_rate, c2, COMPUTATION_FRAC, AhrsIntCmplQuat::correct_gravity, FIR_FILTER_SIZE, FLOAT_VECT3_NORM, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_bias, int32_rmat_of_quat(), INT32_VECT3_RSHIFT, INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_body_quat, AhrsIntCmplQuat::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm_valid, Int32Rates::p, Int64Rates::p, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, RMAT_ELMT, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, VECT3_SMUL, AhrsIntCmplQuat::weight, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_update_gps()

void ahrs_icq_update_gps

(

struct GpsState *

gps_s

)

Definition at line 525 of file ahrs_int_cmpl_quat.c.

References AHRS_HEADING_UPDATE_GPS_MIN_SPEED, ahrs_icq, ahrs_icq_realign_heading(), ahrs_icq_update_heading(), course, GPS_FIX_3D, AhrsIntCmplQuat::heading_aligned, INT32_ANGLE_FRAC, AhrsIntCmplQuat::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm_valid, and SPEED_BFP_OF_REAL.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_update_heading()

void ahrs_icq_update_heading

(

int32_t

heading

)

Update yaw based on a heading measurement.

e.g. from GPS course

Parameters

heading

Heading in body frame, radians (CW/north) with INT32_ANGLE_FRAC

Definition at line 561 of file ahrs_int_cmpl_quat.c.

References AHRS_BIAS_UPDATE_HEADING_THRESHOLD, ahrs_icq, AhrsIntCmplQuat::gyro_bias, heading, AhrsIntCmplQuat::high_rez_bias, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, int32_rmat_of_quat(), int32_rmat_vmult(), INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_body_quat, Int32Rates::p, Int64Rates::p, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, RMAT_ELMT, TRIG_BFP_OF_REAL, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_icq_update_mag()

void ahrs_icq_update_mag	(	struct Int32Vect3 *	mag,
		float	dt
	)

Definition at line 380 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, ahrs_icq_update_mag_2d(), ahrs_icq_update_mag_full(), and AhrsIntCmplQuat::mag_cnt.

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_int_cmpl_quat_SetAccelOmega()

static void ahrs_int_cmpl_quat_SetAccelOmega ( float  omega )

inlinestatic

Definition at line 132 of file ahrs_int_cmpl_quat.h.

References AhrsIntCmplQuat::accel_omega, ahrs_icq, and ahrs_icq_set_accel_gains().

Here is the call graph for this function:

ahrs_int_cmpl_quat_SetAccelZeta()

static void ahrs_int_cmpl_quat_SetAccelZeta ( float  zeta )

inlinestatic

Definition at line 138 of file ahrs_int_cmpl_quat.h.

References AhrsIntCmplQuat::accel_zeta, ahrs_icq, and ahrs_icq_set_accel_gains().

Here is the call graph for this function:

ahrs_int_cmpl_quat_SetMagOmega()

static void ahrs_int_cmpl_quat_SetMagOmega ( float  omega )

inlinestatic

Definition at line 147 of file ahrs_int_cmpl_quat.h.

References ahrs_icq, ahrs_icq_set_mag_gains(), and AhrsIntCmplQuat::mag_omega.

Here is the call graph for this function:

ahrs_int_cmpl_quat_SetMagZeta()

static void ahrs_int_cmpl_quat_SetMagZeta ( float  zeta )

inlinestatic

Definition at line 153 of file ahrs_int_cmpl_quat.h.

Variable Documentation

ahrs_icq

struct AhrsIntCmplQuat ahrs_icq

extern

Default Rate filter Low pass.

Definition at line 1 of file ahrs_int_cmpl_quat.c.

Referenced by accel_cb(), ahrs_icq_align(), ahrs_icq_init(), ahrs_icq_propagate(), ahrs_icq_realign_heading(), ahrs_icq_set_accel_gains(), ahrs_icq_set_mag_gains(), ahrs_icq_update_accel(), ahrs_icq_update_gps(), ahrs_icq_update_heading(), ahrs_icq_update_mag(), ahrs_icq_update_mag_2d(), ahrs_icq_update_mag_full(), ahrs_int_cmpl_quat_SetAccelOmega(), ahrs_int_cmpl_quat_SetAccelZeta(), ahrs_int_cmpl_quat_SetMagOmega(), aligner_cb(), geo_mag_cb(), gyro_cb(), mag_cb(), send_bias(), send_euler(), send_filter_status(), send_geo_mag(), send_quat(), and set_body_state_from_quat().