Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_cmpl.c File Reference

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

#include "subsystems/ahrs/ahrs_float_cmpl.h"
#include "subsystems/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 "subsystems/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 More...
 
#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. More...
 
void ahrs_fc_realign_heading (float heading)
 Hard reset yaw to a heading. More...
 
void ahrs_fc_set_body_to_imu (struct OrientationReps *body_to_imu)
 
void ahrs_fc_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 
void ahrs_fc_recompute_ltp_to_body (void)
 

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

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 57 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_ACCEL_ZETA   0.9

Definition at line 60 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

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

#define AHRS_MAG_OMEGA   0.04

Definition at line 64 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define AHRS_MAG_ZETA   0.9

Definition at line 67 of file ahrs_float_cmpl.c.

Referenced by ahrs_fc_init().

#define FIR_FILTER_SIZE   8

Referenced by ahrs_fc_update_accel().

Function Documentation

bool ahrs_fc_align ( struct FloatRates lp_gyro,
struct FloatVect3 lp_accel,
struct FloatVect3 lp_mag 
)
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 464 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_recompute_ltp_to_body(), AhrsFloatCmpl::body_to_imu, FLOAT_ANGLE_NORMALIZE, float_quat_comp(), float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), float_rmat_of_quat(), AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetQuat_f(), FloatQuat::qi, 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:

void ahrs_fc_recompute_ltp_to_body ( void  )
void ahrs_fc_set_body_to_imu ( struct OrientationReps body_to_imu)

Definition at line 500 of file ahrs_float_cmpl.c.

References ahrs_fc_set_body_to_imu_quat(), and orientationGetQuat_f().

+ Here is the call graph for this function:

void ahrs_fc_set_body_to_imu_quat ( struct FloatQuat q_b2i)

Definition at line 505 of file ahrs_float_cmpl.c.

References ahrs_fc, AhrsFloatCmpl::body_to_imu, AhrsFloatCmpl::is_aligned, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetQuat_f(), orientationGetRMat_f(), and orientationSetQuat_f().

Referenced by ahrs_fc_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_fc_update_gps ( struct GpsState gps_s)

Definition at line 389 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_realign_heading(), ahrs_fc_update_heading(), course, GPS_FIX_3D, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_vel_norm, and AhrsFloatCmpl::ltp_vel_norm_valid.

Referenced by gps_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 420 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_recompute_ltp_to_body(), FLOAT_ANGLE_NORMALIZE, float_rmat_vmult(), AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_body, AhrsFloatCmpl::ltp_to_imu_rmat, orientationGetRMat_f(), AhrsFloatCmpl::rate_correction, 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:

void ahrs_fc_update_mag ( struct FloatVect3 mag,
float  dt 
)

Definition at line 270 of file ahrs_float_cmpl.c.

References ahrs_fc, ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_full(), and AhrsFloatCmpl::mag_cnt.

Referenced by mag_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_fc_update_mag_full ( struct FloatVect3 mag,
float  dt 
)

Variable Documentation