Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 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; \
}
#define TWO_PI_INTEG_EULER
#define 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

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