Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_int_cmpl_euler.c File Reference
#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

◆ AHRS_MAG_OFFSET

#define AHRS_MAG_OFFSET   0.

Definition at line 43 of file ahrs_int_cmpl_euler.c.

◆ F_UPDATE

#define F_UPDATE   512

Definition at line 53 of file ahrs_int_cmpl_euler.c.

◆ FACE_REINJ_1

#define FACE_REINJ_1   1024

Definition at line 39 of file ahrs_int_cmpl_euler.c.

◆ INTEG_EULER_NORMALIZE

#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.

◆ NOISE_FILTER_GAIN

#define NOISE_FILTER_GAIN   50

Definition at line 104 of file ahrs_int_cmpl_euler.c.

◆ PI_INTEG_EULER

#define PI_INTEG_EULER   (INT32_ANGLE_PI * F_UPDATE)

Definition at line 55 of file ahrs_int_cmpl_euler.c.

◆ TWO_PI_INTEG_EULER

#define TWO_PI_INTEG_EULER   (INT32_ANGLE_2_PI * F_UPDATE)

Definition at line 56 of file ahrs_int_cmpl_euler.c.

Function Documentation

◆ ahrs_ice_align()

◆ ahrs_ice_init()

◆ ahrs_ice_propagate()

◆ ahrs_ice_set_body_to_imu()

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(), body_to_imu, and orientationGetQuat_f().

+ Here is the call graph for this function:

◆ ahrs_ice_set_body_to_imu_quat()

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:

◆ ahrs_ice_update_accel()

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:

◆ ahrs_ice_update_mag()

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:

◆ get_phi_theta_measurement_fom_accel()

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, PPRZ_ITRIG_COS, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

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:

◆ get_psi_measurement_from_mag()

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

Variable Documentation

◆ ahrs_ice

TWO_PI_INTEG_EULER
#define TWO_PI_INTEG_EULER
Definition: ahrs_int_cmpl_euler.c:56
PI_INTEG_EULER
#define PI_INTEG_EULER
Definition: ahrs_int_cmpl_euler.c:55