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.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 "state.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. More...
 

Macros

#define AHRS_UNINIT   0
 
#define AHRS_RUNNING   1
 

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)
 Update AHRS state with GPS measurements. More...
 

Variables

struct Ahrs ahrs
 global AHRS state More...
 

Detailed Description

Attitude and Heading Reference System interface.

Definition in file ahrs.h.

Macro Definition Documentation

#define AHRS_RUNNING   1

Definition at line 36 of file ahrs.h.

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

#define AHRS_UNINIT   0

Definition at line 35 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 74 of file ahrs_chimu_spi.c.

References ACCELS_FLOAT_OF_BFP, ahrs, ahrs_aligner, ahrs_float_get_euler_from_accel_mag(), ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ahrs_impl, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), AHRS_RUNNING, bafl_accel_measure, bafl_bias, compute_body_orientation_and_rates(), EULERS_COPY, EULERS_SDIV, F_UPDATE, FALSE, FLOAT_RMAT_OF_EULERS, FLOAT_RMAT_OF_QUAT, get_phi_theta_measurement_fom_accel(), get_psi_measurement_from_mag(), GX3_status, GX3Link, GX3Running, GX3Uninit, AhrsIntCmpl::gyro_bias, AhrsFloatCmpl::gyro_bias, AhrsFloatDCM::gyro_bias, AhrsIntCmplEuler::gyro_bias, AhrsMlkf::gyro_bias, AhrsFloatCmpl::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, AhrsFloatDCM::ltp_to_imu_euler, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsMlkf::ltp_to_imu_rmat, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, Int32Eulers::phi, Int32Eulers::psi, RATES_COPY, RATES_FLOAT_OF_BFP, set_body_orientation_and_rates(), set_body_state_from_euler(), set_body_state_from_quat(), set_dcm_matrix_from_rmat(), Ahrs::status, Int32Eulers::theta, TRUE, and update_ahrs_from_sim().

Referenced by imu_impl_init(), 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 40 of file ahrs_chimu_spi.c.

References ahrs, ahrs_aligner, AHRS_ALIGNER_LOCKED, 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, at_com_send_config(), at_com_send_ftrim(), 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, AhrsFloatCmpl::body_to_imu_quat, AhrsMlkf::body_to_imu_quat, Imu::body_to_imu_quat, AhrsFloatCmpl::body_to_imu_rmat, AhrsMlkf::body_to_imu_rmat, AhrsFloatDCM::body_to_imu_rmat, Imu::body_to_imu_rmat, CHIMU_BROADCAST, CHIMU_Checksum(), CHIMU_Init(), CHIMU_STX, AhrsFloatCmpl::correct_gravity, AhrsIntCmpl::correct_gravity, EULERS_COPY, FACE_REINJ_1, FALSE, FLOAT_QUAT_OF_EULERS, FLOAT_QUAT_ZERO, FLOAT_RATES_ZERO, FLOAT_RMAT_OF_EULERS, 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, AhrsMlkf::gyro_bias, heading, AhrsFloatCmpl::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, AhrsIntCmpl::imu_rate, AhrsFloatCmpl::imu_rate, AhrsFloatDCM::imu_rate, AhrsIntExternQuat::imu_rate, AhrsFloatQuat::imu_rate, AhrsIntCmplEuler::imu_rate, AhrsMlkf::imu_rate, init_at_com(), ins_init(), ins_pitch_neutral, INS_PITCH_NEUTRAL_DEFAULT, ins_roll_neutral, INS_ROLL_NEUTRAL_DEFAULT, InsSend, InsSend1, InsUartSend1, INT32_EULERS_OF_RMAT, INT32_QUAT_ZERO, INT_EULERS_ZERO, INT_RATES_ZERO, LED_ON, AhrsFloatDCM::ltp_to_imu_euler, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_quat, AhrsIntExternQuat::ltp_to_imu_quat, AhrsFloatQuat::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsMlkf::ltp_to_imu_rmat, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsIntCmpl::ltp_vel_norm_valid, MAG_BFP_OF_REAL, AhrsIntCmpl::mag_h, AhrsIntExternQuat::mag_offset, AhrsIntCmplEuler::mag_offset, AhrsFloatQuat::mag_offset, MSG00_PING, MSG09_ESTIMATOR, MSG10_UARTSETTINGS, new_ins_attitude, AhrsMlkf::P, QUAT_COPY, AhrsIntCmpl::rate_correction, AhrsIntCmplEuler::reinj_1, RMAT_COPY, set_dcm_matrix_from_rmat(), Ahrs::status, AhrsAligner::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 137 of file ahrs_chimu_spi.c.

References AhrsARDrone::accel, ahrs_impl, ahrs_lowpass_accel(), AHRS_LTP_TO_BODY, AHRS_PROPAGATE_FREQUENCY, ahrs_sim_available, AHRS_TO_BFP, AhrsARDrone::altitude, _navdata_demo_t::altitude, _navdata_t::ardrone_state, at_com_recieve_navdata(), 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, AhrsARDrone::battery, Imu::body_to_imu_quat, Imu::body_to_imu_rmat, buffer, compute_ahrs_representations(), compute_body_orientation_and_rates(), AhrsARDrone::control_state, _navdata_demo_t::ctrl_state, electrical, AhrsARDrone::eulers, EULERS_ADD, EULERS_DIFF, EULERS_SDIV, F_UPDATE, FALSE, FLOAT_EULERS_OF_RMAT, FLOAT_QUAT_COMP, FLOAT_QUAT_COPY, FLOAT_QUAT_INTEGRATE, FLOAT_QUAT_NORM, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_OF_RMAT, FLOAT_QUAT_SMUL, FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, FLOAT_RMAT_INTEGRATE_FI, FLOAT_RMAT_OF_QUAT, float_rmat_reorthogonalize(), gps_ardrone2_parse(), Imu::gyro, AhrsIntCmpl::gyro_bias, AhrsFloatCmpl::gyro_bias, AhrsFloatDCM::gyro_bias, AhrsIntCmplEuler::gyro_bias, Imu::gyro_prev, _navdata_t::header, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmpl::high_rez_quat, imu, AhrsIntCmpl::imu_rate, AhrsFloatCmpl::imu_rate, AhrsFloatDCM::imu_rate, AhrsIntExternQuat::imu_rate, AhrsIntCmplEuler::imu_rate, INT32_EULERS_DOT_OF_RATES, INT32_QUAT_COMP_INV, INT32_QUAT_INTEGRATE_FI, INT32_QUAT_NORMALIZE, INT32_RMAT_TRANSP_RATEMULT, INT32_VECT3_SCALE_2, INT_RATES_ZERO, INTEG_EULER_NORMALIZE, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_quat, AhrsIntExternQuat::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, Matrix_update(), AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, NAVDATA_HEADER, NOISE_FILTER_GAIN, Normalize(), _navdata_t::options, FloatRates::p, Int32Rates::p, FloatEulers::phi, _navdata_demo_t::phi, _navdata_phys_measures_t::phys_accs, propagate_ref(), propagate_state(), FloatEulers::psi, _navdata_demo_t::psi, Int32Eulers::psi, FloatRates::q, Int32Rates::q, FloatQuat::qi, QUAT_ASSIGN, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, Int32Rates::r, AhrsIntCmpl::rate_correction, AhrsFloatCmpl::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, RMAT_ELMT, set_body_state_from_euler(), set_body_state_from_quat(), _navdata_option_t::size, AhrsARDrone::speed, AhrsARDrone::state, stateSetBodyRates_f(), stateSetBodyRates_i(), stateSetNedToBodyEulers_f(), stateSetNedToBodyQuat_i(), _navdata_option_t::tag, FloatEulers::theta, _navdata_demo_t::theta, TRUE, update_ahrs_from_sim(), _navdata_demo_t::vbat_flying_percentage, Electrical::vsupply, _navdata_demo_t::vx, _navdata_demo_t::vy, _navdata_demo_t::vz, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by on_gyro_accel_event(), on_gyro_event(), and sensors_task().

+ 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 140 of file ahrs_chimu_spi.c.

References Imu::accel, ACCEL_BFP_OF_REAL, ACCELS_FLOAT_OF_BFP, ahrs_do_update_accel(), ahrs_impl, AhrsFloatCmpl::body_to_imu_rmat, Imu::body_to_imu_rmat, COMPUTATION_FRAC, AhrsFloatCmpl::correct_gravity, AhrsIntCmpl::correct_gravity, dn, 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, AhrsFloatCmpl::gyro_bias, AhrsIntCmpl::high_rez_bias, imu, INT32_ACCEL_FRAC, INT32_RATE_FRAC, INT32_RMAT_OF_QUAT, INT32_RMAT_VMULT, INT32_SPEED_FRAC, INT32_VECT3_CROSS_PRODUCT, INT32_VECT3_DIFF, INT32_VECT3_NORM, INT32_VECT3_RSHIFT, INT_RATES_RSHIFT, AhrsMlkf::lp_accel, AhrsIntCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::ltp_vel_norm, AhrsIntCmpl::ltp_vel_norm, AhrsFloatCmpl::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, AhrsFloatCmpl::rate_correction, reset_state(), RMAT_ELMT, stateGetBodyRates_f(), stateGetBodyRates_i(), Int32Eulers::theta, update_state(), 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(), 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_mag ( void  )

Variable Documentation

struct Ahrs ahrs