Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_float_dcm.c File Reference

Attitude estimation for fixedwings based on the DCM Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf. More...

+ Include dependency graph for ahrs_float_dcm.c:

Go to the source code of this file.

Functions

static void compute_ahrs_representations (void)
 
static void compute_body_orientation_and_rates (void)
 
static void set_dcm_matrix_from_rmat (struct FloatRMat *rmat)
 
void Normalize (void)
 
void Drift_correction (void)
 
void Matrix_update (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_gps (void)
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 

Variables

struct AhrsFloatDCM ahrs_impl
 
const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY )
 
struct FloatVect3 accel_float = {0,0,0}
 
float Omega_Vector [3] = {0,0,0}
 
float Omega_P [3] = {0,0,0}
 
float Omega_I [3] = {0,0,0}
 
float Omega [3] = {0,0,0}
 
float DCM_Matrix [3][3] = {{1,0,0},{0,1,0},{0,0,1}}
 
float Update_Matrix [3][3] = {{0,1,2},{3,4,5},{6,7,8}}
 
float Temporary_Matrix [3][3] = {{0,0,0},{0,0,0},{0,0,0}}
 

Detailed Description

Attitude estimation for fixedwings based on the DCM Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf.

Options: -USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present -USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good -USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed

Definition in file ahrs_float_dcm.c.

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 174 of file ahrs_float_dcm.c.

References ahrs, ahrs_aligner, ahrs_float, ahrs_float_get_euler_from_accel_mag(), ahrs_impl, AHRS_RUNNING, compute_body_orientation_and_rates(), FLOAT_QUAT_OF_EULERS, FLOAT_RMAT_OF_QUAT, AhrsFloatDCM::gyro_bias, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsFloat::ltp_to_imu_euler, AhrsFloat::ltp_to_imu_quat, AhrsFloat::ltp_to_imu_rmat, RATES_COPY, RATES_FLOAT_OF_BFP, set_dcm_matrix_from_rmat(), and Ahrs::status.

+ 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 198 of file ahrs_float_dcm.c.

References ahrs_float, ahrs_impl, compute_ahrs_representations(), Imu::gyro, AhrsFloatDCM::gyro_bias, imu, AhrsFloat::imu_rate, Matrix_update(), Normalize(), FloatRates::p, FloatRates::q, FloatRates::r, RATES_DIFF, and RATES_FLOAT_OF_BFP.

+ Here is the call graph for this function:

void ahrs_update_accel ( void  )

Update AHRS state with accerleration measurements.

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

Definition at line 254 of file ahrs_float_dcm.c.

References Imu::accel, ACCELS_FLOAT_OF_BFP, ahrs_impl, Drift_correction(), AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_speed, imu, Omega, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call 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 285 of file ahrs_float_dcm.c.

References ahrs_float, DefaultChannel, DefaultDevice, imu, AhrsFloat::ltp_to_imu_euler, Imu::mag, FloatEulers::phi, FloatEulers::theta, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

static void compute_ahrs_representations ( void  )
inlinestatic
void Matrix_update ( void  )

Definition at line 537 of file ahrs_float_dcm.c.

References ahrs_float, DCM_Matrix, G_Dt, AhrsFloat::imu_rate, Matrix_Multiply(), Omega, Omega_I, Omega_P, Omega_Vector, FloatRates::p, FloatRates::q, FloatRates::r, Temporary_Matrix, Update_Matrix, Vector_Add(), FloatVect3::x, and FloatVect3::y.

Referenced by ahrs_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void Normalize ( void  )

Definition at line 340 of file ahrs_float_dcm.c.

References ahrs_impl, AhrsFloatDCM::body_to_imu_rmat, DCM_Matrix, FALSE, renorm_blowup_count, renorm_sqrt_count, set_dcm_matrix_from_rmat(), TRUE, Vector_Add(), Vector_Cross_Product(), Vector_Dot_Product(), and Vector_Scale().

Referenced by ahrs_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void set_dcm_matrix_from_rmat ( struct FloatRMat rmat)
inlinestatic

Definition at line 125 of file ahrs_float_dcm.c.

References DCM_Matrix, and RMAT_ELMT.

Referenced by ahrs_align(), ahrs_init(), and Normalize().

+ Here is the caller graph for this function:

Variable Documentation

struct FloatVect3 accel_float = {0,0,0}

Definition at line 82 of file ahrs_float_dcm.c.

float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}}
const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY )

Definition at line 80 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

float Omega[3] = {0,0,0}

Definition at line 87 of file ahrs_float_dcm.c.

Referenced by ahrs_update_accel(), and Matrix_update().

float Omega_I[3] = {0,0,0}

Definition at line 86 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

float Omega_P[3] = {0,0,0}

Definition at line 85 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

float Omega_Vector[3] = {0,0,0}

Definition at line 84 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}

Definition at line 91 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}

Definition at line 90 of file ahrs_float_dcm.c.

Referenced by Matrix_update().