Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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_aligner.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "state.h"
#include "subsystems/imu.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_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 
#define AHRS_CORRECT_FREQUENCY   AHRS_PROPAGATE_FREQUENCY
 
#define AHRS_MAG_CORRECT_FREQUENCY   50
 
#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 set_body_state_from_quat (void)
 
static void UNUSED ahrs_update_mag_full (void)
 
static void ahrs_update_mag_2d (void)
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
void ahrs_propagate (void)
 Propagation. More...
 
void ahrs_set_accel_gains (void)
 update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta More...
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 
void ahrs_set_mag_gains (void)
 update pre-computed kp and ki gains from mag_omega and mag_zeta More...
 
void ahrs_update_gps (void)
 Update AHRS state with GPS measurements. More...
 
void ahrs_update_heading (int32_t heading)
 Update yaw based on a heading measurement. More...
 
void ahrs_realign_heading (int32_t heading)
 Hard reset yaw to a heading. More...
 

Variables

struct AhrsIntCmplQuat ahrs_impl
 

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

#define AHRS_ACCEL_OMEGA   0.063

Definition at line 93 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_init().

#define AHRS_ACCEL_ZETA   0.9

Definition at line 96 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_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 120 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_update_heading().

#define AHRS_CORRECT_FREQUENCY   AHRS_PROPAGATE_FREQUENCY

Definition at line 75 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_set_accel_gains().

#define AHRS_GRAVITY_HEURISTIC_FACTOR   30

by default use the gravity heuristic to reduce gain

Definition at line 115 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_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 127 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_update_gps().

#define AHRS_MAG_CORRECT_FREQUENCY   50

Definition at line 83 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_set_mag_gains().

#define AHRS_MAG_OMEGA   0.04

Definition at line 103 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_init().

#define AHRS_MAG_ZETA   0.9

Definition at line 106 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_init().

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 67 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_propagate(), ahrs_set_accel_gains(), and ahrs_set_mag_gains().

#define COMPUTATION_FRAC   16

Referenced by ahrs_update_accel().

#define FIR_FILTER_SIZE   8

Referenced by ahrs_update_accel().

Function Documentation

void ahrs_align ( void  )

Aligns the AHRS.

Called after ahrs_aligner has run to set initial attitude and biases. Must set the ahrs status to AHRS_RUNNING. Needs to be implemented by each AHRS algorithm.

Definition at line 238 of file ahrs_int_cmpl_quat.c.

References ahrs, ahrs_aligner, ahrs_impl, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), AHRS_RUNNING, FALSE, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsIntCmplQuat::high_rez_bias, INT_RATES_LSHIFT, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsIntCmplQuat::ltp_to_imu_quat, RATES_COPY, set_body_state_from_quat(), Ahrs::status, and TRUE.

+ Here is the call graph for this function:

void ahrs_propagate ( void  )

Propagation.

Usually integrates the gyro rates to angles. Reads the global imu data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 262 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, AHRS_PROPAGATE_FREQUENCY, AhrsIntCmplQuat::gyro_bias, Imu::gyro_prev, AhrsIntCmplQuat::high_rez_quat, imu, AhrsIntCmplQuat::imu_rate, INT32_QUAT_INTEGRATE_FI, INT32_QUAT_NORMALIZE, INT_RATES_ZERO, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::rate_correction, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_SDIV, RATES_SMUL, and set_body_state_from_quat().

+ Here is the call graph for this function:

void ahrs_realign_heading ( int32_t  heading)

Hard reset yaw to a heading.

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

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

Definition at line 656 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, Imu::body_to_imu, AhrsIntCmplQuat::heading_aligned, imu, INT32_QUAT_COMP, 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, stateGetNedToBodyQuat_i(), stateSetNedToBodyQuat_i(), and TRUE.

Referenced by ahrs_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_set_accel_gains ( void  )

update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta

Definition at line 292 of file ahrs_int_cmpl_quat.c.

References AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, AhrsIntCmplQuat::accel_omega, AhrsIntCmplQuat::accel_zeta, AHRS_CORRECT_FREQUENCY, ahrs_impl, and AHRS_PROPAGATE_FREQUENCY.

Referenced by ahrs_init(), ahrs_int_cmpl_quat_SetAccelOmega(), and ahrs_int_cmpl_quat_SetAccelZeta().

+ Here is the caller graph for this function:

void ahrs_set_mag_gains ( void  )

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

Definition at line 444 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, AHRS_MAG_CORRECT_FREQUENCY, AHRS_PROPAGATE_FREQUENCY, AhrsIntCmplQuat::mag_ki, AhrsIntCmplQuat::mag_kp, AhrsIntCmplQuat::mag_omega, and AhrsIntCmplQuat::mag_zeta.

Referenced by ahrs_init(), ahrs_int_cmpl_quat_SetMagOmega(), and ahrs_int_cmpl_quat_SetMagZeta().

+ Here is the caller graph for this function:

void ahrs_update_gps ( void  )

Update AHRS state with GPS measurements.

Reads the global gps data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 566 of file ahrs_int_cmpl_quat.c.

References AHRS_HEADING_UPDATE_GPS_MIN_SPEED, ahrs_impl, ahrs_realign_heading(), ahrs_update_heading(), GpsState::cacc, GpsState::course, FALSE, GpsState::fix, gps, GPS_FIX_3D, GpsState::gspeed, AhrsIntCmplQuat::heading_aligned, INT32_ANGLE_FRAC, AhrsIntCmplQuat::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm_valid, GpsState::speed_3d, SPEED_BFP_OF_REAL, and TRUE.

+ Here is the call graph for this function:

void ahrs_update_heading ( int32_t  heading)
void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 436 of file ahrs_int_cmpl_quat.c.

References ahrs_update_mag_2d(), and ahrs_update_mag_full().

+ Here is the call graph for this function:

static void set_body_state_from_quat ( void  )
inlinestatic

Variable Documentation