Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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
#define AHRS_UNINIT   0

Definition at line 35 of file ahrs.h.

Referenced by ahrs_init(), ahrs_propagate(), on_accel_event(), on_gyro_event(), and send_fliter_status().

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

References 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, inv_state::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(), AhrsFloatQuat::gx3_status, GX3Running, GX3Uninit, AhrsFloatDCM::gyro_bias, AhrsFloatCmpl::gyro_bias, AhrsIntCmplQuat::gyro_bias, AhrsMlkf::gyro_bias, AhrsIntCmplEuler::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsFloatCmpl::heading_aligned, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmplQuat::high_rez_bias, ins, ins_impl, INS_RUNNING, INT_RATES_LSHIFT, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsFloatDCM::ltp_to_imu_euler, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsFloatCmpl::ltp_to_imu_quat, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, Int32Eulers::phi, Int32Eulers::psi, inv_state::quat, 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(), InsFloatInv::state, Ahrs::status, Ins::status, Int32Eulers::theta, TRUE, uart_transmit(), and update_ahrs_from_sim().

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

References AhrsFloatCmpl::accel_omega, AhrsIntCmplQuat::accel_omega, AhrsFloatCmpl::accel_zeta, AhrsIntCmplQuat::accel_zeta, ahrs, AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, ahrs_aligner, AHRS_ALIGNER_LOCKED, AHRS_GRAVITY_HEURISTIC_FACTOR, ahrs_impl, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z, AHRS_MAG_OFFSET, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, AHRS_RUNNING, ahrs_set_accel_gains(), ahrs_set_mag_gains(), ahrs_sim_available, AHRS_UNINIT, at_com_send_config(), at_com_send_ftrim(), ImuFloat::body_to_imu, Imu::body_to_imu, CHIMU_BROADCAST, CHIMU_Checksum(), CHIMU_Init(), CHIMU_STX, AhrsFloatCmpl::correct_gravity, AhrsIntCmplQuat::correct_gravity, FACE_REINJ_1, FALSE, FLOAT_RATES_ZERO, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, AhrsFloatDCM::gps_speed, AhrsFloatCmpl::gravity_heuristic_factor, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsIntCmplQuat::gyro_bias, AhrsMlkf::gyro_bias, AhrsIntCmplEuler::gyro_bias, heading, AhrsIntCmplQuat::heading_aligned, AhrsFloatCmpl::heading_aligned, AhrsIntCmplQuat::high_rez_bias, imu, AhrsFloatDCM::imu_rate, AhrsFloatCmpl::imu_rate, AhrsIntCmplQuat::imu_rate, AhrsMlkf::imu_rate, AhrsIntCmplEuler::imu_rate, imuf, init_at_com(), ins_pitch_neutral, INS_PITCH_NEUTRAL_DEFAULT, ins_roll_neutral, INS_ROLL_NEUTRAL_DEFAULT, InsSend, InsSend1, InsUartSend1, INT_RATES_ZERO, AhrsFloatDCM::ltp_to_imu_euler, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatQuat::ltp_to_imu_quat, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsIntCmplQuat::ltp_vel_norm_valid, MAG_BFP_OF_REAL, AhrsFloatCmpl::mag_h, AhrsIntCmplQuat::mag_h, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, AhrsIntCmplEuler::mag_offset, AhrsFloatQuat::mag_offset, AhrsFloatCmpl::mag_omega, AhrsIntCmplQuat::mag_omega, AhrsFloatCmpl::mag_zeta, AhrsIntCmplQuat::mag_zeta, MSG00_PING, MSG09_ESTIMATOR, MSG10_UARTSETTINGS, new_ins_attitude, orientationGetEulers_f(), orientationGetEulers_i(), orientationGetQuat_f(), orientationGetQuat_i(), orientationGetRMat_f(), AhrsMlkf::P, QUAT_COPY, AhrsIntCmplQuat::rate_correction, register_periodic_telemetry(), AhrsIntCmplEuler::reinj_1, send_status(), set_dcm_matrix_from_rmat(), Ahrs::status, AhrsAligner::status, TRUE, 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. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 134 of file ahrs_chimu_spi.c.

References AhrsARDrone::accel, inv_command::accel, Imu::accel, ACCELS_FLOAT_OF_BFP, ahrs, ahrs_impl, AHRS_PROPAGATE_FREQUENCY, ahrs_sim_available, AHRS_UNINIT, AhrsARDrone::altitude, _navdata_demo_t::altitude, _navdata_t::ardrone_state, inv_state::as, at_com_recieve_navdata(), inv_measures::baro_alt, AhrsARDrone::battery, inv_state::bias, Imu::body_to_imu, buffer, InsFloatInv::cmd, compute_ahrs_representations(), compute_body_orientation_and_rates(), AhrsARDrone::control_state, _navdata_demo_t::ctrl_state, DefaultChannel, DefaultDevice, dt, electrical, error_output(), AhrsARDrone::eulers, EULERS_ADD, EULERS_DIFF, EULERS_SDIV, F_UPDATE, FALSE, FLOAT_EULERS_OF_QUAT, FLOAT_QUAT_INTEGRATE, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_OF_RMAT, FLOAT_QUAT_RMAT_B2N, FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, FLOAT_RMAT_INTEGRATE_FI, FLOAT_RMAT_OF_QUAT, float_rmat_reorthogonalize(), FLOAT_VECT3_ADD, FLOAT_VECT3_SMUL, full_read(), gps_ardrone2_parse(), Imu::gyro, AhrsFloatDCM::gyro_bias, AhrsFloatCmpl::gyro_bias, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplEuler::gyro_bias, Imu::gyro_prev, inv_state::hb, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmplQuat::high_rez_quat, imu, AhrsFloatDCM::imu_rate, AhrsFloatCmpl::imu_rate, AhrsIntCmplQuat::imu_rate, AhrsIntCmplEuler::imu_rate, init_invariant_state(), ins, ins_impl, ins_pitch_neutral, ins_roll_neutral, INS_UNINIT, INT32_EULERS_DOT_OF_RATES, INT32_QUAT_INTEGRATE_FI, INT32_QUAT_NORMALIZE, INT32_RMAT_TRANSP_RATEMULT, INT32_RMAT_TRANSP_VMULT, INT32_VECT3_SCALE_2, INT_RATES_ZERO, INTEG_EULER_NORMALIZE, INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), AhrsIntCmplEuler::ltp_to_imu_euler, AhrsFloatCmpl::ltp_to_imu_quat, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, inv_measures::mag, Matrix_update(), InsFloatInv::meas, AhrsIntCmplEuler::measure, AhrsIntCmplEuler::measurement, NAVDATA_HEADER, NOISE_FILTER_GAIN, Normalize(), _navdata_t::options, orientationGetRMat_i(), FloatRates::p, Int32Rates::p, FloatEulers::phi, _navdata_demo_t::phi, _navdata_phys_measures_t::phys_accs, inv_state::pos, inv_measures::pos_gps, propagate_ref(), propagate_state(), FloatEulers::psi, _navdata_demo_t::psi, Int32Eulers::psi, FloatRates::q, Int32Rates::q, FloatQuat::qi, inv_state::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, Int32Rates::r, AhrsFloatCmpl::rate_correction, AhrsIntCmplQuat::rate_correction, RATE_FLOAT_OF_BFP, inv_command::rates, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_FLOAT_OF_BFP, RATES_SDIV, RATES_SMUL, RATES_SUB, RATES_SUM, RATES_SUM_SCALED, AhrsIntCmplEuler::reinj_1, InsFloatInv::reset, AhrsIntCmplEuler::residual, runge_kutta_4_float(), set_body_state_from_euler(), set_body_state_from_quat(), _navdata_option_t::size, AhrsARDrone::speed, inv_state::speed, inv_measures::speed_gps, AhrsARDrone::state, InsFloatInv::state, stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyEulers_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), Ahrs::status, Ins::status, _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, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by 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. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 137 of file ahrs_chimu_spi.c.

References ACC_FROM_CROSS_FRAC, Imu::accel, AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, AhrsFloatCmpl::accel_omega, AhrsFloatCmpl::accel_zeta, ACCELS_FLOAT_OF_BFP, AHRS_CORRECT_FREQUENCY, ahrs_impl, AHRS_PROPAGATE_FREQUENCY, Imu::body_to_imu, COMPUTATION_FRAC, AhrsFloatCmpl::correct_gravity, AhrsIntCmplQuat::correct_gravity, dn, Drift_correction(), FIR_FILTER_SIZE, 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, AhrsFloatCmpl::gravity_heuristic_factor, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsFloatCmpl::gyro_bias, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_bias, imu, INT32_RMAT_OF_QUAT, INT32_RMAT_VMULT, INT32_VECT3_CROSS_PRODUCT, INT32_VECT3_DIFF, INT32_VECT3_RSHIFT, INT_RATES_RSHIFT, AhrsMlkf::lp_accel, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsIntCmplQuat::ltp_vel_norm_valid, AhrsIntCmplEuler::measurement, NOISE_FILTER_GAIN, Omega, orientationGetRMat_f(), orientationGetRMat_i(), Int32Rates::p, Int64Rates::p, Int32Eulers::phi, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsFloatCmpl::rate_correction, AhrsIntCmplQuat::rate_correction, reset_state(), RMAT_ELMT, stateGetBodyRates_f(), stateGetBodyRates_i(), Int32Eulers::theta, update_state(), VECT3_ADD, VECT3_COPY, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, VECT3_SMUL, VECT3_SUM_SCALED, AhrsIntCmplQuat::weight, AhrsFloatCmpl::weight, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by on_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_gps ( void  )

Update AHRS state with GPS measurements.

Reads the global gps data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 112 of file ahrs_chimu_spi.c.

References AHRS_HEADING_UPDATE_GPS_MIN_SPEED, ahrs_impl, ahrs_realign_heading(), ahrs_update_heading(), UtmCoor_f::alt, GpsState::cacc, CHIMU_Checksum(), GpsState::course, UtmCoor_f::east, UtmCoor_i::east, ECEF_FLOAT_OF_BFP, GpsState::ecef_pos, GpsState::ecef_vel, FALSE, GpsState::fix, FloatSwap, gps, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, GPS_FIX_3D, AhrsFloatDCM::gps_speed, GpsState::gspeed, heading, AhrsIntCmplQuat::heading_aligned, AhrsFloatCmpl::heading_aligned, GpsState::hmsl, ins, ins_gps_fix_once, ins_impl, INS_RUNNING, InsSend, INT32_ANGLE_FRAC, AhrsFloatCmpl::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm, AhrsFloatCmpl::ltp_vel_norm_valid, AhrsIntCmplQuat::ltp_vel_norm_valid, InsFloatInv::meas, State::ned_initialized_f, ned_of_ecef_point_f(), ned_of_ecef_vect_f(), State::ned_origin_f, GpsState::ned_vel, UtmCoor_f::north, UtmCoor_i::north, inv_measures::pos_gps, GpsState::speed_3d, SPEED_BFP_OF_REAL, inv_measures::speed_gps, state, stateGetHorizontalWindspeed_f(), Ins::status, TRUE, State::utm_initialized_f, State::utm_origin_f, GpsState::utm_pos, FloatVect2::x, NedCoor_f::x, NedCoor_i::x, FloatVect2::y, NedCoor_f::y, NedCoor_i::y, NedCoor_f::z, and NedCoor_i::z.

Referenced by on_gps_event(), and on_gps_solution().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation