Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_int_cmpl_quat.c File Reference

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

#include "generated/airframe.h"
#include "modules/ahrs/ahrs_int_cmpl_quat.h"
#include "modules/ahrs/ahrs_int_utils.h"
#include "modules/gps/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
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD   5.0
 don't update gyro bias if heading deviation is above this threshold in degrees
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED   5.0
 Minimum speed in m/s for heading update via GPS.
#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
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
void ahrs_icq_update_gps (struct GpsState *gps_s)
void ahrs_icq_update_heading (int32_t heading)
 Update yaw based on a heading measurement.
void ahrs_icq_realign_heading (int32_t heading)
 Hard reset yaw to a heading.

Variables

struct AhrsIntCmplQuat ahrs_icq
 Default Rate filter Low pass.

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

Referenced by ahrs_icq_update_accel().

◆ AHRS_ACCEL_OMEGA

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 66 of file ahrs_int_cmpl_quat.c.

◆ AHRS_ACCEL_ZETA

#define AHRS_ACCEL_ZETA   0.9

Definition at line 69 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 93 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_icq_update_heading().

◆ AHRS_GRAVITY_HEURISTIC_FACTOR

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

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

Referenced by ahrs_icq_update_gps().

◆ AHRS_MAG_OMEGA

#define AHRS_MAG_OMEGA   0.04

Definition at line 76 of file ahrs_int_cmpl_quat.c.

◆ AHRS_MAG_ZETA

#define AHRS_MAG_ZETA   0.9

Definition at line 79 of file ahrs_int_cmpl_quat.c.

◆ COMPUTATION_FRAC

#define COMPUTATION_FRAC   16

Referenced by ahrs_icq_update_accel().

◆ 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 )

Definition at line 161 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, AHRS_ICQ_RUNNING, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), INT_RATES_LSHIFT, and RATES_COPY.

Referenced by aligner_cb(), and aligner_cb().

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

◆ ahrs_icq_init()

void ahrs_icq_init ( void )

◆ ahrs_icq_propagate()

void ahrs_icq_propagate ( struct Int32Rates * gyro,
float dt )

Definition at line 190 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, int32_quat_integrate_fi(), int32_quat_normalize(), INT_RATES_ZERO, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_SDIV, and RATES_SMUL.

Referenced by gyro_cb(), and gyro_cb().

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

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

References ahrs_icq, heading, int32_quat_comp_norm_shortest(), int32_quat_inv_comp_norm_shortest(), int32_quat_normalize(), 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 223 of file ahrs_int_cmpl_quat.c.

References 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_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 397 of file ahrs_int_cmpl_quat.c.

References ahrs_icq.

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()

void ahrs_icq_update_accel ( struct Int32Vect3 * accel,
float dt )

◆ ahrs_icq_update_gps()

void ahrs_icq_update_gps ( struct GpsState * gps_s)

Definition at line 525 of file ahrs_int_cmpl_quat.c.

References AHRS_HEADING_UPDATE_GPS_MIN_SPEED, ahrs_icq, ahrs_icq_realign_heading(), ahrs_icq_update_heading(), course, GPS_FIX_3D, INT32_ANGLE_FRAC, and SPEED_BFP_OF_REAL.

Referenced by gps_cb(), and gps_cb().

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

◆ ahrs_icq_update_heading()

void ahrs_icq_update_heading ( int32_t heading)

Update yaw based on a heading measurement.

e.g. from GPS course

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

Definition at line 561 of file ahrs_int_cmpl_quat.c.

References AHRS_BIAS_UPDATE_HEADING_THRESHOLD, ahrs_icq, heading, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, int32_rmat_of_quat(), int32_rmat_vmult(), INT_RATES_RSHIFT, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, RMAT_ELMT, TRIG_BFP_OF_REAL, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_update_gps().

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

◆ ahrs_icq_update_mag()

void ahrs_icq_update_mag ( struct Int32Vect3 * mag,
float dt )

Definition at line 380 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, ahrs_icq_update_mag_2d(), and ahrs_icq_update_mag_full().

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()

void ahrs_icq_update_mag_2d ( struct Int32Vect3 * mag,
float dt )
inlinestatic

Definition at line 459 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, INT32_MAG_FRAC, int32_rmat_of_quat(), int32_rmat_transp_vmult(), int32_rmat_vmult(), int32_vect2_normalize(), INT_RATES_RSHIFT, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_update_mag().

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

◆ ahrs_icq_update_mag_full()

void ahrs_icq_update_mag_full ( struct Int32Vect3 * mag,
float dt )
inlinestatic

Definition at line 406 of file ahrs_int_cmpl_quat.c.

References ahrs_icq, int32_rmat_of_quat(), int32_rmat_vmult(), INT_RATES_RSHIFT, VECT3_CROSS_PRODUCT, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_update_mag().

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

Variable Documentation

◆ ahrs_icq