Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_int_cmpl_quat.c File Reference
#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 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
 Default Rate filter Low pass. More...
 

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

◆ ACC_FROM_CROSS_FRAC

#define ACC_FROM_CROSS_FRAC   INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC

◆ AHRS_ACCEL_OMEGA

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 62 of file ahrs_int_cmpl_quat.c.

◆ AHRS_ACCEL_ZETA

#define AHRS_ACCEL_ZETA   0.9

Definition at line 65 of file ahrs_int_cmpl_quat.c.

◆ AHRS_BIAS_UPDATE_HEADING_THRESHOLD

#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 89 of file ahrs_int_cmpl_quat.c.

◆ AHRS_GRAVITY_HEURISTIC_FACTOR

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 84 of file ahrs_int_cmpl_quat.c.

◆ AHRS_HEADING_UPDATE_GPS_MIN_SPEED

#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 96 of file ahrs_int_cmpl_quat.c.

◆ AHRS_MAG_OMEGA

#define AHRS_MAG_OMEGA   0.04

Definition at line 72 of file ahrs_int_cmpl_quat.c.

◆ AHRS_MAG_ZETA

#define AHRS_MAG_ZETA   0.9

Definition at line 75 of file ahrs_int_cmpl_quat.c.

◆ COMPUTATION_FRAC

#define COMPUTATION_FRAC   16

◆ FIR_FILTER_SIZE

#define FIR_FILTER_SIZE   8

Function Documentation

◆ ahrs_icq_align()

bool ahrs_icq_align ( struct Int32Rates lp_gyro,
struct Int32Vect3 lp_accel,
struct Int32Vect3 lp_mag 
)

◆ ahrs_icq_init()

◆ ahrs_icq_propagate()

void ahrs_icq_propagate ( struct Int32Rates gyro,
float  dt 
)

◆ ahrs_icq_realign_heading()

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 627 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AhrsIntCmplQuat::body_to_imu, heading, 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, and Int32Quat::qz.

Referenced by ahrs_icq_update_gps().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ ahrs_icq_set_accel_gains()

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

◆ ahrs_icq_set_body_to_imu()

void ahrs_icq_set_body_to_imu ( struct OrientationReps body_to_imu)

Definition at line 662 of file ahrs_int_cmpl_quat.c.

References ahrs_icq_set_body_to_imu_quat(), body_to_imu, and orientationGetQuat_f().

+ Here is the call graph for this function:

◆ ahrs_icq_set_body_to_imu_quat()

void ahrs_icq_set_body_to_imu_quat ( struct FloatQuat q_b2i)

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

◆ ahrs_icq_set_mag_gains()

void ahrs_icq_set_mag_gains ( void  )

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

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

◆ ahrs_icq_update_accel()

◆ ahrs_icq_update_gps()

void ahrs_icq_update_gps ( struct GpsState gps_s)

◆ ahrs_icq_update_heading()

◆ ahrs_icq_update_mag()

void ahrs_icq_update_mag ( struct Int32Vect3 mag,
float  dt 
)

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

◆ ahrs_icq_update_mag_2d()

◆ ahrs_icq_update_mag_full()

Variable Documentation

◆ ahrs_icq