Paparazzi UAS  v4.0.4_stable-3-gf39211a
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 "std.h"
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
#include "math/pprz_algebra_float.h"
#include <string.h>
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
+ Include dependency graph for ahrs_float_dcm.c:

Go to the source code of this file.

Macros

#define DOWNLINK_DEVICE   DOWNLINK_AP_DEVICE
 

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_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)
 

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.

Definition in file ahrs_float_dcm.c.

Macro Definition Documentation

#define DOWNLINK_DEVICE   DOWNLINK_AP_DEVICE

Definition at line 42 of 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 160 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 184 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 215 of file ahrs_float_dcm.c.

References Imu::accel, ACCELS_FLOAT_OF_BFP, Drift_correction(), GpsState::fix, gps, GPS_FIX_3D, imu, Omega, GpsState::speed_3d, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call graph for this function:

void ahrs_update_gps ( void  )

Definition at line 291 of file ahrs_float_dcm.c.

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 238 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 477 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 295 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 117 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 74 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 72 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

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

Definition at line 79 of file ahrs_float_dcm.c.

Referenced by ahrs_update_accel(), and Matrix_update().

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

Definition at line 78 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

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

Definition at line 77 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

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

Definition at line 76 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 83 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 82 of file ahrs_float_dcm.c.

Referenced by Matrix_update().