|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Complementary filter in float to estimate the attitude, heading and gyro bias. More...
#include "std.h"#include "math/pprz_algebra_float.h"#include "math/pprz_orientation_conversion.h"#include "modules/gps/gps.h"Go to the source code of this file.
Data Structures | |
| struct | AhrsFloatCmpl |
Enumerations | |
| enum | AhrsFCStatus { AHRS_FC_UNINIT , AHRS_FC_RUNNING } |
Functions | |
| void | ahrs_fc_init (void) |
| bool | ahrs_fc_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag) |
| void | ahrs_fc_propagate (struct FloatRates *gyro, float dt) |
| void | ahrs_fc_update_accel (struct FloatVect3 *accel, float dt) |
| void | ahrs_fc_update_mag (struct FloatVect3 *mag, float dt) |
| void | ahrs_fc_update_gps (struct GpsState *gps_s) |
| void | ahrs_fc_update_heading (float heading) |
| Update yaw based on a heading measurement. | |
| void | ahrs_fc_realign_heading (float heading) |
| Hard reset yaw to a heading. | |
Variables | |
| struct AhrsFloatCmpl | ahrs_fc |
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.h.
| struct AhrsFloatCmpl |
Definition at line 43 of file ahrs_float_cmpl.h.
| Data Fields | ||
|---|---|---|
| uint16_t | accel_cnt | number of propagations since last accel update |
| float | accel_omega | filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) |
| float | accel_zeta | filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) |
| struct FloatRates | body_rate | |
| bool | correct_gravity | enable gravity correction during coordinated turns |
| uint8_t | gravity_heuristic_factor |
sets how strongly the gravity heuristic reduces accel correction. Set to zero in order to disable gravity heuristic. |
| struct FloatRates | gyro_bias | |
| bool | heading_aligned | |
| bool | is_aligned | |
| struct FloatQuat | ltp_to_body_quat | |
| struct FloatRMat | ltp_to_body_rmat | |
| float | ltp_vel_norm | velocity norm for gravity correction during coordinated turns |
| bool | ltp_vel_norm_valid | |
| uint16_t | mag_cnt | number of propagations since last mag update |
| struct FloatVect3 | mag_h | |
| float | mag_omega | filter cut-off frequency for correcting the attitude (heading) from magnetometer |
| float | mag_zeta | filter damping for correcting the gyro bias from magnetometer |
| struct FloatRates | rate_correction | |
| enum AhrsFCStatus | status | |
| float | weight | |
| enum AhrsFCStatus |
| Enumerator | |
|---|---|
| AHRS_FC_UNINIT | |
| AHRS_FC_RUNNING | |
Definition at line 38 of file ahrs_float_cmpl.h.
|
extern |
Definition at line 115 of file ahrs_float_cmpl.c.
References ahrs_fc, AHRS_FC_RUNNING, ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), and float_rmat_of_quat().
Referenced by aligner_cb().
|
extern |
Definition at line 82 of file ahrs_float_cmpl.c.
References AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, ahrs_fc, AHRS_FC_UNINIT, AHRS_GRAVITY_HEURISTIC_FACTOR, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, float_quat_identity(), FLOAT_RATES_ZERO, float_rmat_identity(), and VECT3_ASSIGN.
Referenced by ahrs_fc_wrapper_init().
|
extern |
Definition at line 142 of file ahrs_float_cmpl.c.
References ahrs_fc, alpha, float_quat_integrate(), float_quat_normalize(), float_quat_of_rmat(), FLOAT_RATES_LIN_CMB, FLOAT_RATES_ZERO, float_rmat_integrate_fi(), float_rmat_of_quat(), float_rmat_reorthogonalize(), RATES_COPY, RATES_SUB, and RATES_SUM.
Referenced by gyro_cb().
| void ahrs_fc_realign_heading | ( | float | heading | ) |
Hard reset yaw to a heading.
Doesn't affect the bias. Sets ahrs_fc.heading_aligned to TRUE.
| heading | Heading in body frame, radians (CW/north) |
Definition at line 460 of file ahrs_float_cmpl.c.
References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), float_rmat_of_quat(), heading, FloatQuat::qi, QUAT_COPY, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.
Referenced by ahrs_fc_update_gps().
|
extern |
Definition at line 178 of file ahrs_float_cmpl.c.
References ahrs_fc, c2, FIR_FILTER_SIZE, float_vect3_norm(), RATES_ADD_SCALED_VECT, RMAT_ELMT, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, and VECT3_SMUL.
Referenced by accel_cb().
|
extern |
Definition at line 388 of file ahrs_float_cmpl.c.
References ahrs_fc, ahrs_fc_realign_heading(), ahrs_fc_update_heading(), course, and GPS_FIX_3D.
Referenced by gps_cb().
| void ahrs_fc_update_heading | ( | float | heading | ) |
Update yaw based on a heading measurement.
e.g. from GPS course
| heading | Heading in body frame, radians (CW/north) |
Definition at line 419 of file ahrs_float_cmpl.c.
References ahrs_fc, FLOAT_ANGLE_NORMALIZE, float_rmat_vmult(), heading, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatVect2::x, FloatVect2::y, and FloatVect3::z.
Referenced by ahrs_fc_update_gps().
|
extern |
Definition at line 269 of file ahrs_float_cmpl.c.
References ahrs_fc, ahrs_fc_update_mag_2d(), and ahrs_fc_update_mag_full().
Referenced by mag_cb().
|
extern |
Definition at line 80 of file ahrs_float_cmpl.c.
Referenced by accel_cb(), ahrs_fc_align(), ahrs_fc_init(), ahrs_fc_propagate(), ahrs_fc_realign_heading(), ahrs_fc_update_accel(), ahrs_fc_update_gps(), ahrs_fc_update_heading(), ahrs_fc_update_mag(), ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_2d_dumb(), ahrs_fc_update_mag_full(), aligner_cb(), compute_body_orientation_and_rates(), geo_mag_cb(), gyro_cb(), mag_cb(), send_bias(), send_euler(), send_euler_int(), send_filter_status(), and send_geo_mag().