Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_int_cmpl_quat.c File Reference

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

#include "generated/airframe.h"
#include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/gps.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
+ Include dependency graph for ahrs_int_cmpl_quat.c:

Go to the source code of this file.

Macros

#define AHRS_ACCEL_OMEGA   0.063
 
#define AHRS_ACCEL_ZETA   0.9
 
#define AHRS_MAG_OMEGA   0.04
 
#define AHRS_MAG_ZETA   0.9
 
#define AHRS_GRAVITY_HEURISTIC_FACTOR   30
 by default use the gravity heuristic to reduce gain More...
 
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD   5.0
 don't update gyro bias if heading deviation is above this threshold in degrees More...
 
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED   5.0
 Minimum speed in m/s for heading update via GPS. More...
 
#define COMPUTATION_FRAC   16
 
#define ACC_FROM_CROSS_FRAC   INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
 
#define FIR_FILTER_SIZE   8
 

Functions

static void UNUSED ahrs_icq_update_mag_full (struct Int32Vect3 *mag, float dt)
 
static void ahrs_icq_update_mag_2d (struct Int32Vect3 *mag, float dt)
 
void ahrs_icq_init (void)
 
bool_t 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_set_accel_gains (void)
 update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta More...
 
void ahrs_icq_update_accel (struct Int32Vect3 *accel, float dt)
 
void ahrs_icq_update_mag (struct Int32Vect3 *mag, float dt)
 
void ahrs_icq_set_mag_gains (void)
 update pre-computed kp and ki gains from mag_omega and mag_zeta More...
 
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_body_to_imu (struct OrientationReps *body_to_imu)
 
void ahrs_icq_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 

Variables

struct AhrsIntCmplQuat ahrs_icq
 

Detailed Description

Quaternion complementary filter (fixed-point).

Estimate the attitude, heading and gyro bias.

Definition in file ahrs_int_cmpl_quat.c.

Macro Definition Documentation

#define ACC_FROM_CROSS_FRAC   INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC

Referenced by ahrs_icq_update_accel().

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 66 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_init().

#define AHRS_ACCEL_ZETA   0.9

Definition at line 69 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_init().

#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD   5.0

don't update gyro bias if heading deviation is above this threshold in degrees

Definition at line 93 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_update_heading().

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 88 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_init().

#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED   5.0

Minimum speed in m/s for heading update via GPS.

Don't update heading from GPS course if GPS ground speed is below is this threshold

Definition at line 100 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_update_gps().

#define AHRS_MAG_OMEGA   0.04

Definition at line 76 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_init().

#define AHRS_MAG_ZETA   0.9

Definition at line 79 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_init().

#define COMPUTATION_FRAC   16

Referenced by ahrs_icq_update_accel().

#define FIR_FILTER_SIZE   8

Referenced by ahrs_icq_update_accel().

Function Documentation

bool_t 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_realign_heading ( int32_t  heading)

Hard reset yaw to a heading.

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

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

Definition at line 612 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AhrsIntCmplQuat::body_to_imu, AhrsIntCmplQuat::heading_aligned, int32_quat_comp(), int32_quat_comp_inv(), int32_quat_comp_norm_shortest(), int32_quat_inv_comp_norm_shortest(), int32_quat_normalize(), AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, QUAT_COPY, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and TRUE.

Referenced by ahrs_icq_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 213 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:

void ahrs_icq_set_body_to_imu ( struct OrientationReps body_to_imu)

Definition at line 647 of file ahrs_int_cmpl_quat.c.

References ahrs_icq_set_body_to_imu_quat(), and orientationGetQuat_f().

+ Here is the call graph for this function:

void ahrs_icq_set_body_to_imu_quat ( struct FloatQuat q_b2i)

Definition at line 652 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AhrsIntCmplQuat::body_to_imu, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), and orientationSetQuat_f().

Referenced by ahrs_icq_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_icq_set_mag_gains ( void  )

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

Definition at line 386 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(), ahrs_int_cmpl_quat_SetMagOmega(), and ahrs_int_cmpl_quat_SetMagZeta().

+ Here is the caller graph for this function:

void ahrs_icq_update_gps ( struct GpsState gps_s)
void ahrs_icq_update_mag ( struct Int32Vect3 mag,
float  dt 
)

Definition at line 369 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:

Variable Documentation