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_float_dcm.c File Reference

Attitude estimation for fixedwings based on the DCM. 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 "firmwares/fixedwing/autopilot.h"
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "subsystems/gps.h"
#include <string.h>
#include "led.h"
+ Include dependency graph for ahrs_float_dcm.c:

Go to the source code of this file.

Macros

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 
#define INS_ROLL_NEUTRAL_DEFAULT   0
 
#define INS_PITCH_NEUTRAL_DEFAULT   0
 

Functions

static void compute_ahrs_representations (void)
 
static void set_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)
 Update AHRS state with GPS measurements. 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...
 

Variables

float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT
 
float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT
 
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}}
 
float MAG_Heading_X = 1
 
float MAG_Heading_Y = 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.

Macro Definition Documentation

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 62 of file ahrs_float_dcm.c.

#define INS_PITCH_NEUTRAL_DEFAULT   0

Definition at line 71 of file ahrs_float_dcm.c.

Referenced by ahrs_init(), ArduIMU_init(), ins_init(), and xsens_init().

#define INS_ROLL_NEUTRAL_DEFAULT   0

Definition at line 68 of file ahrs_float_dcm.c.

Referenced by ahrs_init(), ArduIMU_init(), ins_init(), and xsens_init().

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

References ahrs, ahrs_aligner, ahrs_float_get_euler_from_accel_mag(), ahrs_impl, AHRS_RUNNING, FLOAT_RMAT_OF_EULERS, AhrsFloatDCM::gyro_bias, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsFloatDCM::ltp_to_imu_euler, RATES_COPY, RATES_FLOAT_OF_BFP, set_body_orientation_and_rates(), 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 194 of file ahrs_float_dcm.c.

References ahrs_impl, compute_ahrs_representations(), Imu::gyro, AhrsFloatDCM::gyro_bias, imu, AhrsFloatDCM::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 250 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_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 225 of file ahrs_float_dcm.c.

References ahrs_impl, GpsState::course, FALSE, GpsState::fix, gps, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, GPS_FIX_3D, AhrsFloatDCM::gps_speed, GpsState::gspeed, GpsState::speed_3d, and TRUE.

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

References ahrs_impl, DefaultChannel, DefaultDevice, imu, AhrsFloatDCM::ltp_to_imu_euler, Imu::mag, MAG_Heading_X, MAG_Heading_Y, FloatEulers::phi, FloatEulers::theta, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

static void compute_ahrs_representations ( void  )
inlinestatic

Definition at line 597 of file ahrs_float_dcm.c.

References ahrs_impl, DCM_Matrix, GRAVITY, AhrsFloatDCM::ltp_to_imu_euler, FloatEulers::phi, FloatEulers::psi, set_body_orientation_and_rates(), FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void Matrix_update ( void  )

Definition at line 534 of file ahrs_float_dcm.c.

References ahrs_impl, DCM_Matrix, G_Dt, AhrsFloatDCM::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 336 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_body_orientation_and_rates ( void  )
inlinestatic
static void set_dcm_matrix_from_rmat ( struct FloatRMat rmat)
inlinestatic

Definition at line 130 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 87 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 85 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT

Definition at line 74 of file ahrs_float_dcm.c.

Referenced by set_body_orientation_and_rates().

float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT
Todo:
harmonize infrared neutrals with ins_neutrals or get rid of ins neutrals this ins only needed for sim right now

Definition at line 73 of file ahrs_float_dcm.c.

Referenced by set_body_orientation_and_rates().

float MAG_Heading_X = 1

Definition at line 99 of file ahrs_float_dcm.c.

Referenced by ahrs_update_mag(), and Drift_correction().

float MAG_Heading_Y = 0

Definition at line 100 of file ahrs_float_dcm.c.

Referenced by ahrs_update_mag(), and Drift_correction().

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

Definition at line 92 of file ahrs_float_dcm.c.

Referenced by ahrs_update_accel(), and Matrix_update().

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

Definition at line 91 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

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

Definition at line 90 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

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

Definition at line 89 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 96 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 95 of file ahrs_float_dcm.c.

Referenced by Matrix_update().