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.h File Reference

Attitude and Heading Reference System interface. More...

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
+ Include dependency graph for ahrs.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  Ahrs
 Attitude and Heading Reference System state (fixed point version) More...
 
struct  AhrsFloat
 Attitude and Heading Reference System state (floating point version) More...
 

Macros

#define AHRS_UNINIT   0
 
#define AHRS_RUNNING   1
 
#define AHRS_FLOAT_OF_INT32()
 
#define AHRS_INT_OF_FLOAT()
 
#define AHRS_IMU_INT_OF_FLOAT()
 

Functions

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 Ahrs ahrs
 global AHRS state (fixed point version) More...
 
struct AhrsFloat ahrs_float
 global AHRS state (floating point version) More...
 
float ahrs_mag_offset
 

Detailed Description

Attitude and Heading Reference System interface.

Definition in file ahrs.h.

Macro Definition Documentation

#define AHRS_FLOAT_OF_INT32 ( )
Value:
{ \
}
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
struct Int32Quat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:49
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:605
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:619
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:569
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69

Definition at line 83 of file ahrs.h.

#define AHRS_IMU_INT_OF_FLOAT ( )
Value:
{ \
}
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:59
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:612
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:61
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:581
struct Int32Rates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:47
struct Int32Quat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:44
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:625
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:62
struct Int32Eulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:45
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:575
struct Int32RMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:46
struct FloatEulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:60

Definition at line 96 of file ahrs.h.

Referenced by ahrs_align(), ahrs_propagate(), and ahrs_realign_heading().

#define AHRS_INT_OF_FLOAT ( )
Value:
{ \
}
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
struct FloatRMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:68
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:612
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:581
struct Int32RMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:51
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:625
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
struct Int32Quat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:49
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:575

Definition at line 89 of file ahrs.h.

Referenced by ahrs_align(), ahrs_propagate(), and ahrs_realign_heading().

#define AHRS_RUNNING   1

Definition at line 34 of file ahrs.h.

Referenced by ahrs_align(), ahrs_init(), ahrs_is_aligned(), and on_mag_event().

#define AHRS_UNINIT   0

Definition at line 33 of file ahrs.h.

Referenced by ahrs_init(), on_accel_event(), on_gyro_accel_event(), and on_gyro_event().

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 100 of file ahrs_float_cmpl.c.

References ACCELS_FLOAT_OF_BFP, ahrs, ahrs_aligner, ahrs_float, ahrs_float_get_euler_from_accel_mag(), ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ahrs_impl, AHRS_IMU_INT_OF_FLOAT, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), AHRS_INT_OF_FLOAT, AHRS_RUNNING, bafl_accel_measure, bafl_bias, compute_body_orientation(), compute_body_orientation_and_rates(), compute_imu_euler_and_rmat_from_quat(), compute_imu_quat_and_rmat_from_euler(), compute_imu_rmat_and_euler_from_quat(), EULERS_COPY, EULERS_SDIV, F_UPDATE, FALSE, FLOAT_QUAT_OF_EULERS, FLOAT_RMAT_OF_QUAT, get_phi_theta_measurement_fom_accel(), get_psi_measurement_from_mag(), AhrsIntCmpl::gyro_bias, AhrsFloatCmplRmat::gyro_bias, AhrsFloatDCM::gyro_bias, AhrsIntCmplEuler::gyro_bias, AhrsFloatCmplRmat::heading_aligned, AhrsIntCmpl::heading_aligned, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmpl::high_rez_bias, INT_RATES_LSHIFT, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsFloat::ltp_to_imu_euler, Ahrs::ltp_to_imu_euler, AhrsFloat::ltp_to_imu_quat, Ahrs::ltp_to_imu_quat, AhrsFloat::ltp_to_imu_rmat, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, Int32Eulers::phi, Int32Eulers::psi, RATES_COPY, RATES_FLOAT_OF_BFP, set_dcm_matrix_from_rmat(), Ahrs::status, Int32Eulers::theta, TRUE, and update_ahrs_from_sim().

Referenced by on_gyro_accel_event(), and on_gyro_event().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_init ( void  )

AHRS initialization.

Called at startup. Needs to be implemented by each AHRS algorithm.

Definition at line 75 of file ins_xsens.c.

References ahrs, ahrs_float, ahrs_float_lkf_SetRaccel, ahrs_float_lkf_SetRmag, AHRS_H_X, AHRS_H_Y, ahrs_impl, ahrs_mag_offset, AHRS_RUNNING, ahrs_sim_available, AHRS_UNINIT, bafl_h, BAFL_hx, BAFL_hy, BAFL_hz, bafl_P, BAFL_Q_ATT, bafl_Q_att, BAFL_Q_GYRO, bafl_Q_gyro, bafl_quat, BAFL_SIGMA_ACCEL, BAFL_SIGMA_MAG, BAFL_SSIZE, bafl_T, AhrsFloat::body_rate, Ahrs::body_rate, AhrsFloatCmplRmat::body_to_imu_quat, Imu::body_to_imu_quat, AhrsFloatDCM::body_to_imu_quat, AhrsFloatCmplRmat::body_to_imu_rmat, Imu::body_to_imu_rmat, AhrsFloatDCM::body_to_imu_rmat, AhrsFloatCmplRmat::correct_gravity, AhrsIntCmpl::correct_gravity, EULERS_COPY, FACE_REINJ_1, FALSE, FLOAT_EULERS_ZERO, FLOAT_QUAT_OF_EULERS, FLOAT_QUAT_ZERO, FLOAT_RATES_ZERO, FLOAT_RMAT_OF_EULERS, FLOAT_RMAT_ZERO, FLOAT_VECT3_ASSIGN, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, AhrsFloatDCM::gps_speed, AhrsIntCmpl::gyro_bias, AhrsIntCmplEuler::gyro_bias, AhrsFloatCmplRmat::heading_aligned, AhrsIntCmpl::heading_aligned, high_accel_done, high_accel_flag, AhrsIntCmpl::high_rez_bias, imu, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_PSI, IMU_BODY_TO_IMU_THETA, AhrsFloat::imu_rate, Ahrs::imu_rate, ins_init(), INT32_EULERS_OF_RMAT, INT32_QUAT_ZERO, INT32_RMAT_ZERO, INT_EULERS_ZERO, INT_RATES_ZERO, AhrsFloat::ltp_to_body_euler, Ahrs::ltp_to_body_euler, AhrsFloat::ltp_to_body_quat, Ahrs::ltp_to_body_quat, AhrsFloat::ltp_to_body_rmat, Ahrs::ltp_to_body_rmat, AhrsFloat::ltp_to_imu_euler, Ahrs::ltp_to_imu_euler, AhrsFloat::ltp_to_imu_quat, Ahrs::ltp_to_imu_quat, AhrsFloat::ltp_to_imu_rmat, Ahrs::ltp_to_imu_rmat, AhrsFloatCmplRmat::ltp_vel_norm_valid, AhrsIntCmpl::ltp_vel_norm_valid, MAG_BFP_OF_REAL, AhrsIntCmpl::mag_h, QUAT_COPY, AhrsIntCmpl::rate_correction, RATES_COPY, AhrsIntCmplEuler::reinj_1, RMAT_COPY, set_dcm_matrix_from_rmat(), Ahrs::status, TRUE, AhrsIntCmpl::use_gravity_heuristic, and VECT3_ASSIGN.

Referenced by init_ap(), and main_init().

+ Here is the call graph for this function:

+ Here is the caller 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 131 of file ahrs_float_cmpl.c.

References ahrs, ahrs_float, ahrs_impl, AHRS_IMU_INT_OF_FLOAT, AHRS_INT_OF_FLOAT, ahrs_lowpass_accel(), AHRS_LTP_TO_BODY, AHRS_PROPAGATE_FREQUENCY, AHRS_TO_BFP, bafe_bias, BAFE_DT, bafe_F, bafe_qdot, bafe_quat, bafe_rates, bafl_bias, bafl_dcm, BAFL_DT, bafl_eulers, bafl_P, bafl_Q_att, bafl_Q_gyro, bafl_qnorm, bafl_qr, bafl_qtemp, bafl_quat, bafl_rates, BAFL_SSIZE, bafl_T, bafl_tempP, AhrsFloat::body_rate, compute_ahrs_representations(), compute_body_orientation(), compute_body_orientation_and_rates(), compute_imu_euler_and_rmat_from_quat(), compute_imu_quat_and_euler_from_rmat(), compute_imu_quat_and_rmat_from_euler(), compute_imu_rmat_and_euler_from_quat(), dt, EULERS_ADD, EULERS_DIFF, EULERS_SDIV, F_UPDATE, FLOAT_EULERS_OF_RMAT, FLOAT_QUAT_COMP, FLOAT_QUAT_COPY, FLOAT_QUAT_INTEGRATE, FLOAT_QUAT_NORM, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_SMUL, FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, FLOAT_RMAT_INTEGRATE_FI, FLOAT_RMAT_OF_QUAT, float_rmat_reorthogonalize(), Imu::gyro, AhrsIntCmpl::gyro_bias, AhrsFloatCmplRmat::gyro_bias, AhrsFloatDCM::gyro_bias, AhrsIntCmplEuler::gyro_bias, Imu::gyro_prev, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmpl::high_rez_quat, imu, AhrsFloat::imu_rate, Ahrs::imu_rate, INT32_EULERS_DOT_OF_RATES, INT32_QUAT_INTEGRATE_FI, INT32_QUAT_NORMALIZE, INT_RATES_ZERO, INTEG_EULER_NORMALIZE, Ahrs::ltp_to_imu_euler, AhrsFloat::ltp_to_imu_quat, Ahrs::ltp_to_imu_quat, AhrsFloat::ltp_to_imu_rmat, Matrix_update(), AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, NOISE_FILTER_GAIN, Normalize(), FloatRates::p, Int32Rates::p, Int32Eulers::psi, FloatRates::q, Int32Rates::q, FloatQuat::qi, QUAT_ASSIGN, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, Int32Rates::r, AhrsIntCmpl::rate_correction, AhrsFloatCmplRmat::rate_correction, RATE_FLOAT_OF_BFP, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_FLOAT_OF_BFP, RATES_SDIV, RATES_SMUL, RATES_SUB, RATES_SUM, RATES_SUM_SCALED, AhrsIntCmplEuler::reinj_1, AhrsIntCmplEuler::residual, and RMAT_ELMT.

Referenced by on_gyro_accel_event(), and on_gyro_event().

+ Here is the call graph for this function:

+ Here is the caller 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 170 of file ahrs_float_cmpl.c.

References Imu::accel, ACCEL_BFP_OF_REAL, ACCELS_FLOAT_OF_BFP, ahrs, ahrs_do_update_accel(), ahrs_float, ahrs_impl, AhrsFloat::body_rate, Ahrs::body_rate, AhrsFloatCmplRmat::body_to_imu_rmat, Imu::body_to_imu_rmat, COMPUTATION_FRAC, AhrsFloatCmplRmat::correct_gravity, AhrsIntCmpl::correct_gravity, Drift_correction(), FLOAT_RATES_ADD_SCALED_VECT, FLOAT_RMAT_VECT3_MUL, FLOAT_VECT3_CROSS_PRODUCT, FLOAT_VECT3_NORM, get_phi_theta_measurement_fom_accel(), AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_speed, AhrsIntCmpl::gyro_bias, AhrsFloatCmplRmat::gyro_bias, AhrsIntCmpl::high_rez_bias, imu, INT32_ACCEL_FRAC, INT32_RATE_FRAC, INT32_RMAT_VMULT, INT32_SPEED_FRAC, INT32_VECT3_CROSS_PRODUCT, INT32_VECT3_DIFF, INT32_VECT3_NORM, INT32_VECT3_RSHIFT, INT_RATES_RSHIFT, AhrsFloat::ltp_to_imu_rmat, Ahrs::ltp_to_imu_rmat, AhrsFloatCmplRmat::ltp_vel_norm, AhrsIntCmpl::ltp_vel_norm, AhrsFloatCmplRmat::ltp_vel_norm_valid, AhrsIntCmpl::ltp_vel_norm_valid, AhrsIntCmplEuler::measurement, NOISE_FILTER_GAIN, Omega, Int32Rates::p, Int64Rates::p, Int32Eulers::phi, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmpl::rate_correction, AhrsFloatCmplRmat::rate_correction, RMAT_ELMT, Int32Eulers::theta, AhrsIntCmpl::use_gravity_heuristic, VECT3_ADD, VECT3_COPY, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, VECT3_SMUL, VECT3_SUM_SCALED, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by ahrs_update(), on_accel_event(), and on_gyro_accel_event().

+ 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 228 of file ahrs_float_cmpl.c.

References ahrs, ahrs_do_update_mag(), ahrs_float, ahrs_impl, ahrs_update_mag_2d(), ahrs_update_mag_full(), DefaultChannel, DefaultDevice, get_psi_measurement_from_mag(), imu, AhrsFloat::ltp_to_imu_euler, Ahrs::ltp_to_imu_euler, Imu::mag, AhrsIntCmplEuler::measurement, FloatEulers::phi, Int32Eulers::phi, Int32Eulers::psi, FloatEulers::theta, Int32Eulers::theta, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by ahrs_update(), and on_mag_event().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

float ahrs_mag_offset

Definition at line 27 of file ahrs.c.

Referenced by ahrs_init().