ahrs_madgwick.c File Reference

#include "modules/ahrs/ahrs_madgwick.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include "subsystems/ahrs/ahrs_float_utils.h"

Include dependency graph for ahrs_madgwick.c:

Go to the source code of this file.

Macros
#define	AHRS_MADGWICK_BETA   0.1f

Functions
static void	init_state (void)
void	ahrs_madgwick_init (void)
void	ahrs_madgwick_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
void	ahrs_madgwick_propagate (struct FloatRates *gyro, float dt)
void	ahrs_madgwick_update_accel (struct FloatVect3 *accel)
void	ahrs_madgwick_set_body_to_imu_quat (struct FloatQuat *q_b2i)

Variables
struct AhrsMadgwick	ahrs_madgwick

Macro Definition Documentation

AHRS_MADGWICK_BETA

#define AHRS_MADGWICK_BETA   0.1f

Definition at line 35 of file ahrs_madgwick.c.

Function Documentation

ahrs_madgwick_align()

void ahrs_madgwick_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel
	)

Definition at line 59 of file ahrs_madgwick.c.

References ahrs_float_get_quat_from_accel(), ahrs_madgwick, AhrsMadgwick::bias, AhrsMadgwick::is_aligned, and AhrsMadgwick::quat.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_madgwick_init()

void ahrs_madgwick_init

(

void

)

Definition at line 50 of file ahrs_madgwick.c.

References ahrs_madgwick, init_state(), AhrsMadgwick::is_aligned, and AhrsMadgwick::reset.

Referenced by ahrs_madgwick_register().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_madgwick_propagate()

void ahrs_madgwick_propagate	(	struct FloatRates *	gyro,
		float	dt
	)

Definition at line 69 of file ahrs_madgwick.c.

References AhrsMadgwick::accel, ahrs_madgwick, AHRS_MADGWICK_BETA, AhrsMadgwick::bias, AhrsMadgwick::body_to_imu, f, float_quat_derivative(), float_quat_normalize(), float_rmat_transp_ratemult(), float_vect3_norm(), init_state(), AhrsMadgwick::is_aligned, orientationGetRMat_f(), FloatQuat::qi, AhrsMadgwick::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, AhrsMadgwick::rates, RATES_DIFF, AhrsMadgwick::reset, logger_uart_parse::s1, logger_uart_parse::s2, logger_uart_parse::s3, VECT3_SDIV, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_madgwick_set_body_to_imu_quat()

void ahrs_madgwick_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 147 of file ahrs_madgwick.c.

References ahrs_madgwick, AhrsMadgwick::body_to_imu, AhrsMadgwick::is_aligned, orientationSetQuat_f(), and AhrsMadgwick::quat.

Referenced by body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_madgwick_update_accel()

void ahrs_madgwick_update_accel

(

struct FloatVect3 *

accel

)

Definition at line 142 of file ahrs_madgwick.c.

References AhrsMadgwick::accel, and ahrs_madgwick.

Referenced by accel_cb().

Here is the caller graph for this function:

init_state()

static void init_state ( void  )

inlinestatic

Definition at line 43 of file ahrs_madgwick.c.

References AhrsMadgwick::accel, ahrs_madgwick, AhrsMadgwick::bias, float_quat_identity(), FLOAT_RATES_ZERO, FLOAT_VECT3_ZERO, and AhrsMadgwick::quat.

Referenced by ahrs_madgwick_init(), and ahrs_madgwick_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

ahrs_madgwick

struct AhrsMadgwick ahrs_madgwick

Definition at line 39 of file ahrs_madgwick.c.

Referenced by accel_cb(), ahrs_madgwick_align(), ahrs_madgwick_init(), ahrs_madgwick_propagate(), ahrs_madgwick_set_body_to_imu_quat(), ahrs_madgwick_update_accel(), aligner_cb(), compute_body_orientation_and_rates(), gyro_cb(), init_state(), send_att(), and send_filter_status().