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

Complementary filter in float to estimate the attitude, heading and gyro bias. More...

#include "modules/ahrs/ahrs_float_cmpl.h"
#include "modules/ahrs/ahrs_float_utils.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"
#include "modules/gps/gps.h"
Include dependency graph for ahrs_float_cmpl.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 FIR_FILTER_SIZE   8

Functions

void ahrs_fc_update_mag_full (struct FloatVect3 *mag, float dt)
void ahrs_fc_update_mag_2d (struct FloatVect3 *mag, float dt)
void ahrs_fc_update_mag_2d_dumb (struct FloatVect3 *mag)
void ahrs_fc_init (void)
bool ahrs_fc_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ahrs_fc_propagate (struct FloatRates *gyro, float dt)
void ahrs_fc_update_accel (struct FloatVect3 *accel, float dt)
void ahrs_fc_update_mag (struct FloatVect3 *mag, float dt)
void ahrs_fc_update_gps (struct GpsState *gps_s)
void ahrs_fc_update_heading (float heading)
 Update yaw based on a heading measurement.
void ahrs_fc_realign_heading (float heading)
 Hard reset yaw to a heading.

Variables

struct AhrsFloatCmpl ahrs_fc

Detailed Description

Complementary filter in float to estimate the attitude, heading and gyro bias.

Propagation can be done in rotation matrix or quaternion representation.

Definition in file ahrs_float_cmpl.c.

Macro Definition Documentation

◆ AHRS_ACCEL_OMEGA

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 57 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init(), and ahrs_icq_init().

◆ AHRS_ACCEL_ZETA

#define AHRS_ACCEL_ZETA   0.9

Definition at line 60 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init(), and ahrs_icq_init().

◆ AHRS_GRAVITY_HEURISTIC_FACTOR

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 72 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init(), and ahrs_icq_init().

◆ AHRS_MAG_OMEGA

#define AHRS_MAG_OMEGA   0.04

Definition at line 64 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init(), and ahrs_icq_init().

◆ AHRS_MAG_ZETA

#define AHRS_MAG_ZETA   0.9

Definition at line 67 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init(), and ahrs_icq_init().

◆ FIR_FILTER_SIZE

#define FIR_FILTER_SIZE   8

Function Documentation

◆ ahrs_fc_align()

bool ahrs_fc_align ( struct FloatRates * lp_gyro,
struct FloatVect3 * lp_accel,
struct FloatVect3 * lp_mag )

Definition at line 115 of file ahrs_float_cmpl.c.

References ahrs_fc, AHRS_FC_RUNNING, ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), and float_rmat_of_quat().

Referenced by aligner_cb().

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

◆ ahrs_fc_init()

void ahrs_fc_init ( void )

Definition at line 82 of file ahrs_float_cmpl.c.

References AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, ahrs_fc, AHRS_FC_UNINIT, AHRS_GRAVITY_HEURISTIC_FACTOR, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, float_quat_identity(), FLOAT_RATES_ZERO, float_rmat_identity(), and VECT3_ASSIGN.

Referenced by ahrs_fc_wrapper_init().

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

◆ ahrs_fc_propagate()

void ahrs_fc_propagate ( struct FloatRates * gyro,
float dt )

Definition at line 142 of file ahrs_float_cmpl.c.

References ahrs_fc, alpha, float_quat_integrate(), float_quat_normalize(), float_quat_of_rmat(), FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, float_rmat_integrate_fi(), float_rmat_of_quat(), float_rmat_reorthogonalize(), RATES_COPY, RATES_SUB, and RATES_SUM.

Referenced by gyro_cb().

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

◆ ahrs_fc_realign_heading()

void ahrs_fc_realign_heading ( float heading)

Hard reset yaw to a heading.

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

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

Definition at line 460 of file ahrs_float_cmpl.c.

References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), float_rmat_of_quat(), heading, FloatQuat::qi, QUAT_COPY, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by ahrs_fc_update_gps().

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

◆ ahrs_fc_update_accel()

void ahrs_fc_update_accel ( struct FloatVect3 * accel,
float dt )

Definition at line 178 of file ahrs_float_cmpl.c.

References ahrs_fc, c2, FIR_FILTER_SIZE, float_vect3_norm(), RATES_ADD_SCALED_VECT, RMAT_ELMT, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, and VECT3_SMUL.

Referenced by accel_cb().

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

◆ ahrs_fc_update_gps()

void ahrs_fc_update_gps ( struct GpsState * gps_s)

Definition at line 388 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_realign_heading(), ahrs_fc_update_heading(), course, and GPS_FIX_3D.

Referenced by gps_cb().

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

◆ ahrs_fc_update_heading()

void ahrs_fc_update_heading ( float heading)

Update yaw based on a heading measurement.

e.g. from GPS course

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

Definition at line 419 of file ahrs_float_cmpl.c.

References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_rmat_vmult(), heading, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatVect2::x, FloatVect2::y, and FloatVect3::z.

Referenced by ahrs_fc_update_gps().

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

◆ ahrs_fc_update_mag()

void ahrs_fc_update_mag ( struct FloatVect3 * mag,
float dt )

Definition at line 269 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_update_mag_2d(), and ahrs_fc_update_mag_full().

Referenced by mag_cb().

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

◆ ahrs_fc_update_mag_2d()

void ahrs_fc_update_mag_2d ( struct FloatVect3 * mag,
float dt )

Definition at line 316 of file ahrs_float_cmpl.c.

References ahrs_fc, float_rmat_transp_vmult(), float_rmat_vmult(), float_vect2_normalize(), RATES_ADD_SCALED_VECT, VECT2_COPY, FloatVect2::x, FloatVect3::x, FloatVect2::y, and FloatVect3::y.

Referenced by ahrs_fc_update_mag().

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

◆ ahrs_fc_update_mag_2d_dumb()

void ahrs_fc_update_mag_2d_dumb ( struct FloatVect3 * mag)

Definition at line 361 of file ahrs_float_cmpl.c.

References ahrs_fc, float_eulers_of_rmat(), FloatEulers::phi, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ ahrs_fc_update_mag_full()

void ahrs_fc_update_mag_full ( struct FloatVect3 * mag,
float dt )

Definition at line 286 of file ahrs_float_cmpl.c.

References ahrs_fc, float_rmat_vmult(), RATES_ADD_SCALED_VECT, and VECT3_CROSS_PRODUCT.

Referenced by ahrs_fc_update_mag().

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

Variable Documentation

◆ ahrs_fc