ahrs_int_cmpl_euler.c File Reference

Complementary filter in euler representation (fixed-point). More...

#include "ahrs_int_cmpl_euler.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"

Include dependency graph for ahrs_int_cmpl_euler.c:

Go to the source code of this file.

Macros
#define	FACE_REINJ_1   1024
#define	AHRS_MAG_OFFSET   0.
#define	F_UPDATE   512
#define	PI_INTEG_EULER   (INT32_ANGLE_PI * F_UPDATE)
#define	TWO_PI_INTEG_EULER   (INT32_ANGLE_2_PI * F_UPDATE)
#define	INTEG_EULER_NORMALIZE(_a)
#define	NOISE_FILTER_GAIN   50

Functions
static void	get_phi_theta_measurement_fom_accel (int32_t *phi_meas, int32_t *theta_meas, struct Int32Vect3 *accel)
static void	get_psi_measurement_from_mag (int32_t *psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 *mag)
void	ahrs_ice_init (void)
bool	ahrs_ice_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ahrs_ice_propagate (struct Int32Rates *gyro)
void	ahrs_ice_update_accel (struct Int32Vect3 *accel)
void	ahrs_ice_update_mag (struct Int32Vect3 *mag)
void	ahrs_ice_set_body_to_imu (struct OrientationReps *body_to_imu)
void	ahrs_ice_set_body_to_imu_quat (struct FloatQuat *q_b2i)

Variables
struct AhrsIntCmplEuler	ahrs_ice

Detailed Description

Complementary filter in euler representation (fixed-point).

Estimate the attitude, heading and gyro bias.

Definition in file ahrs_int_cmpl_euler.c.

Macro Definition Documentation

#define AHRS_MAG_OFFSET   0.

Definition at line 43 of file ahrs_int_cmpl_euler.c.

Referenced by ahrs_ice_init().

#define F_UPDATE   512

Definition at line 53 of file ahrs_int_cmpl_euler.c.

Referenced by ahrs_ice_align(), ahrs_ice_propagate(), get_phi_theta_measurement_fom_accel(), and get_psi_measurement_from_mag().

#define FACE_REINJ_1   1024

Definition at line 39 of file ahrs_int_cmpl_euler.c.

Referenced by ahrs_ice_init().

#define INTEG_EULER_NORMALIZE

(

_a

)

Value:

{                         \
    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER; \
    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER; \
  }

Definition at line 57 of file ahrs_int_cmpl_euler.c.

Referenced by ahrs_ice_propagate().

#define NOISE_FILTER_GAIN   50

Definition at line 104 of file ahrs_int_cmpl_euler.c.

Referenced by ahrs_ice_propagate(), and ahrs_ice_update_accel().

#define PI_INTEG_EULER   (INT32_ANGLE_PI * F_UPDATE)

Definition at line 55 of file ahrs_int_cmpl_euler.c.

#define TWO_PI_INTEG_EULER   (INT32_ANGLE_2_PI * F_UPDATE)

Definition at line 56 of file ahrs_int_cmpl_euler.c.

Function Documentation

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

Definition at line 78 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, AHRS_ICE_RUNNING, EULERS_COPY, EULERS_SDIV, F_UPDATE, get_phi_theta_measurement_fom_accel(), get_psi_measurement_from_mag(), AhrsIntCmplEuler::gyro_bias, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmplEuler::is_aligned, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, Int32Eulers::phi, Int32Eulers::psi, RATES_COPY, AhrsIntCmplEuler::status, and Int32Eulers::theta.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_ice_init

(

void

)

Definition at line 63 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, AHRS_ICE_UNINIT, AHRS_MAG_OFFSET, FACE_REINJ_1, AhrsIntCmplEuler::gyro_bias, AhrsIntCmplEuler::imu_rate, INT_EULERS_ZERO, INT_RATES_ZERO, AhrsIntCmplEuler::is_aligned, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmplEuler::mag_offset, AhrsIntCmplEuler::reinj_1, and AhrsIntCmplEuler::status.

Referenced by ahrs_ice_register().

Here is the caller graph for this function:

void ahrs_ice_propagate

(

struct Int32Rates *

gyro

)

Definition at line 154 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, EULERS_ADD, EULERS_DIFF, EULERS_SDIV, F_UPDATE, AhrsIntCmplEuler::gyro_bias, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmplEuler::imu_rate, int32_eulers_dot_of_rates, INTEG_EULER_NORMALIZE, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, NOISE_FILTER_GAIN, Int32Eulers::psi, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_SDIV, RATES_SUM_SCALED, AhrsIntCmplEuler::reinj_1, and AhrsIntCmplEuler::residual.

Referenced by gyro_cb().

Here is the caller graph for this function:

void ahrs_ice_set_body_to_imu

(

struct OrientationReps *

body_to_imu

)

Definition at line 277 of file ahrs_int_cmpl_euler.c.

References ahrs_ice_set_body_to_imu_quat(), and orientationGetQuat_f().

Here is the call graph for this function:

void ahrs_ice_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 282 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, AhrsIntCmplEuler::body_to_imu, AhrsIntCmplEuler::is_aligned, AhrsIntCmplEuler::ltp_to_imu_euler, orientationGetEulers_i(), and orientationSetQuat_f().

Referenced by ahrs_ice_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_ice_update_accel

(

struct Int32Vect3 *

accel

)

Definition at line 202 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, get_phi_theta_measurement_fom_accel(), AhrsIntCmplEuler::measurement, NOISE_FILTER_GAIN, Int32Eulers::phi, Int32Eulers::theta, VECT3_COPY, VECT3_SDIV, and VECT3_SUM_SCALED.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_ice_update_mag

(

struct Int32Vect3 *

mag

)

Definition at line 224 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, get_psi_measurement_from_mag(), AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmplEuler::measurement, Int32Eulers::phi, Int32Eulers::psi, and Int32Eulers::theta.

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

static void get_phi_theta_measurement_fom_accel ( int32_t *  phi_meas, int32_t *  theta_meas, struct Int32Vect3 *  accel  )

inlinestatic

Definition at line 233 of file ahrs_int_cmpl_euler.c.

References F_UPDATE, int32_atan2(), INT32_TRIG_FRAC, INT_MULT_RSHIFT, and PPRZ_ITRIG_COS.

Referenced by ahrs_ice_align(), and ahrs_ice_update_accel().

Here is the call graph for this function:

Here is the caller graph for this function:

static void get_psi_measurement_from_mag ( int32_t *  psi_meas, int32_t  phi_est, int32_t  theta_est, struct Int32Vect3 *  mag  )

inlinestatic

Definition at line 248 of file ahrs_int_cmpl_euler.c.

References ahrs_ice, F_UPDATE, INT32_ANGLE_FRAC, INT32_TRIG_FRAC, AhrsIntCmplEuler::mag_offset, PPRZ_ITRIG_COS, and PPRZ_ITRIG_SIN.

Referenced by ahrs_ice_align(), and ahrs_ice_update_mag().

Here is the caller graph for this function:

Variable Documentation

struct AhrsIntCmplEuler ahrs_ice

Definition at line 46 of file ahrs_int_cmpl_euler.c.

Referenced by accel_cb(), ahrs_ice_align(), ahrs_ice_init(), ahrs_ice_propagate(), ahrs_ice_set_body_to_imu_quat(), ahrs_ice_update_accel(), ahrs_ice_update_mag(), aligner_cb(), get_psi_measurement_from_mag(), gyro_cb(), mag_cb(), send_bias(), send_euler(), send_filter(), send_filter_status(), and set_body_state_from_euler().