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

Complementary filter in float to estimate the attitude, heading and gyro bias. More...

+ Include dependency graph for ahrs_float_cmpl.c:

Go to the source code of this file.

Macros

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 

Functions

void ahrs_update_mag_full (void)
 
void ahrs_update_mag_2d (void)
 
void ahrs_update_mag_2d_dumb (void)
 
static void compute_body_orientation_and_rates (void)
 Compute body orientation and rates from imu orientation and rates. More...
 
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...
 
void ahrs_update_heading (float heading)
 Update yaw based on a heading measurement. More...
 
void ahrs_realign_heading (float heading)
 Hard reset yaw to a heading. More...
 

Variables

struct AhrsFloatCmpl ahrs_impl
 

Detailed Description

Complementary filter in float to estimate the attitude, heading and gyro bias.

Propagation can be done in rotation matrix or quaternion representation.

Definition in file ahrs_float_cmpl.c.

Macro Definition Documentation

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 62 of file ahrs_float_cmpl.c.

Referenced by ahrs_propagate().

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

References ahrs, ahrs_aligner, ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ahrs_impl, AHRS_RUNNING, compute_body_orientation_and_rates(), FALSE, FLOAT_RMAT_OF_QUAT, AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::heading_aligned, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, RATES_COPY, RATES_FLOAT_OF_BFP, Ahrs::status, and TRUE.

+ Here is the call graph for this function:

void ahrs_realign_heading ( float  heading)

Hard reset yaw to a heading.

Doesn't affect the bias. Sets ahrs_impl.heading_aligned to TRUE.

Parameters
headingHeading in body frame, radians (CW/north)

Definition at line 387 of file ahrs_float_cmpl.c.

References ahrs_impl, AhrsFloatCmpl::body_to_imu_quat, FLOAT_ANGLE_NORMALIZE, FLOAT_QUAT_COMP, FLOAT_QUAT_COMP_NORM_SHORTEST, FLOAT_QUAT_INV_COMP_NORM_SHORTEST, FLOAT_QUAT_NORMALIZE, FLOAT_RMAT_OF_QUAT, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_to_imu_quat, AhrsFloatCmpl::ltp_to_imu_rmat, FloatQuat::qi, QUAT_COPY, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, stateGetNedToBodyQuat_f(), stateSetNedToBodyQuat_f(), and TRUE.

Referenced by ahrs_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_update_accel ( void  )
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 317 of file ahrs_float_cmpl.c.

References ahrs_impl, ahrs_realign_heading(), ahrs_update_heading(), GpsState::cacc, GpsState::course, FALSE, GpsState::fix, gps, GPS_FIX_3D, GpsState::gspeed, AhrsFloatCmpl::heading_aligned, AhrsFloatCmpl::ltp_vel_norm, AhrsFloatCmpl::ltp_vel_norm_valid, GpsState::speed_3d, and TRUE.

+ Here is the call graph for this function:

void ahrs_update_heading ( float  heading)

Update yaw based on a heading measurement.

e.g. from GPS course

Parameters
headingHeading in body frame, radians (CW/north)

Definition at line 349 of file ahrs_float_cmpl.c.

References ahrs_impl, FLOAT_ANGLE_NORMALIZE, FLOAT_RATES_ADD_SCALED_VECT, FLOAT_RMAT_VECT3_MUL, AhrsFloatCmpl::gyro_bias, AhrsFloatCmpl::ltp_to_imu_rmat, AhrsFloatCmpl::rate_correction, RMAT_ELMT, stateGetNedToBodyRMat_f(), FloatVect2::x, FloatVect2::y, and FloatVect3::z.

Referenced by ahrs_update_gps().

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

References ahrs_update_mag_2d(), and ahrs_update_mag_full().

+ Here is the call graph for this function:

static void ahrs_update_mag_full ( void  )
inline
static void compute_body_orientation_and_rates ( void  )
inlinestatic

Compute body orientation and rates from imu orientation and rates.

Definition at line 427 of file ahrs_float_cmpl.c.

References ahrs_impl, AhrsFloatCmpl::body_to_imu_quat, AhrsFloatCmpl::body_to_imu_rmat, FLOAT_QUAT_COMP_INV, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_OF_EULERS, FLOAT_RMAT_TRANSP_RATEMULT, AhrsFloatCmpl::imu_rate, ins_pitch_neutral, ins_roll_neutral, AhrsFloatCmpl::ltp_to_imu_quat, stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

Referenced by ahrs_align(), and ahrs_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation