Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 "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 "generated/airframe.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 COMPUTATION_FRAC   16
 

Functions

static void ahrs_update_mag_full (void)
 
static void ahrs_update_mag_2d (void)
 
static void set_body_state_from_quat (void)
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
void ahrs_propagate (void)
 Propagation. 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_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 AhrsIntCmpl 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 AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 60 of file ahrs_int_cmpl_quat.c.

Referenced by ahrs_propagate().

#define COMPUTATION_FRAC   16

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 110 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, AhrsIntCmpl::gyro_bias, AhrsIntCmpl::heading_aligned, AhrsIntCmpl::high_rez_bias, INT_RATES_LSHIFT, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsIntCmpl::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. Needs to be implemented by each AHRS algorithm.

Definition at line 139 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, AHRS_PROPAGATE_FREQUENCY, AhrsIntCmpl::gyro_bias, Imu::gyro_prev, AhrsIntCmpl::high_rez_quat, imu, AhrsIntCmpl::imu_rate, INT32_QUAT_INTEGRATE_FI, INT32_QUAT_NORMALIZE, INT_RATES_ZERO, AhrsIntCmpl::ltp_to_imu_quat, AhrsIntCmpl::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 427 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, Imu::body_to_imu_quat, AhrsIntCmpl::heading_aligned, imu, INT32_QUAT_COMP, INT32_QUAT_COMP_NORM_SHORTEST, INT32_QUAT_INV_COMP_NORM_SHORTEST, INT32_QUAT_NORMALIZE, AhrsIntCmpl::ltp_to_imu_quat, 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_update_gps ( void  )

Update AHRS state with GPS measurements.

Reads the global gps data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 338 of file ahrs_int_cmpl_quat.c.

References ahrs_impl, ahrs_realign_heading(), ahrs_update_heading(), GpsState::cacc, GpsState::course, FALSE, GpsState::fix, gps, GPS_FIX_3D, GpsState::gspeed, AhrsIntCmpl::heading_aligned, INT32_ANGLE_FRAC, AhrsIntCmpl::ltp_vel_norm, AhrsIntCmpl::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)

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

References ahrs_impl, AhrsIntCmpl::gyro_bias, AhrsIntCmpl::high_rez_bias, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, INT32_RMAT_OF_QUAT, INT32_RMAT_VMULT, INT_RATES_RSHIFT, AhrsIntCmpl::ltp_to_imu_quat, Int32Rates::p, Int64Rates::p, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmpl::rate_correction, RMAT_ELMT, stateGetNedToBodyRMat_i(), TRIG_BFP_OF_REAL, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 257 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