|
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 "modules/ahrs/ahrs_float_cmpl.h"#include "modules/ahrs/ahrs_float_utils.h"#include "math/pprz_algebra_float.h"#include "math/pprz_algebra_int.h"#include "math/pprz_simple_matrix.h"#include "generated/airframe.h"#include "modules/gps/gps.h"Go to the source code of this file.
Macros | |
| #define | AHRS_ACCEL_OMEGA 0.063 |
| #define | AHRS_ACCEL_ZETA 0.9 |
| #define | AHRS_MAG_OMEGA 0.04 |
| #define | AHRS_MAG_ZETA 0.9 |
| #define | AHRS_GRAVITY_HEURISTIC_FACTOR 30 |
| by default use the gravity heuristic to reduce gain | |
| #define | FIR_FILTER_SIZE 8 |
Functions | |
| void | ahrs_fc_update_mag_full (struct FloatVect3 *mag, float dt) |
| void | ahrs_fc_update_mag_2d (struct FloatVect3 *mag, float dt) |
| void | ahrs_fc_update_mag_2d_dumb (struct FloatVect3 *mag) |
| 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.c.
| #define AHRS_ACCEL_OMEGA 0.063 |
Definition at line 57 of file ahrs_float_cmpl.c.
Referenced by ahrs_fc_init(), and ahrs_icq_init().
| #define AHRS_ACCEL_ZETA 0.9 |
Definition at line 60 of file ahrs_float_cmpl.c.
Referenced by ahrs_fc_init(), and ahrs_icq_init().
| #define AHRS_GRAVITY_HEURISTIC_FACTOR 30 |
by default use the gravity heuristic to reduce gain
Definition at line 72 of file ahrs_float_cmpl.c.
Referenced by ahrs_fc_init(), and ahrs_icq_init().
| #define AHRS_MAG_OMEGA 0.04 |
Definition at line 64 of file ahrs_float_cmpl.c.
Referenced by ahrs_fc_init(), and ahrs_icq_init().
| #define AHRS_MAG_ZETA 0.9 |
Definition at line 67 of file ahrs_float_cmpl.c.
Referenced by ahrs_fc_init(), and ahrs_icq_init().
| #define FIR_FILTER_SIZE 8 |
Referenced by ahrs_fc_update_accel(), and ahrs_icq_update_accel().
| bool ahrs_fc_align | ( | struct FloatRates * | lp_gyro, |
| struct FloatVect3 * | lp_accel, | ||
| struct FloatVect3 * | lp_mag ) |
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().
| void ahrs_fc_init | ( | void | ) |
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().
| void ahrs_fc_propagate | ( | struct FloatRates * | gyro, |
| float | dt ) |
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().
| void ahrs_fc_update_accel | ( | struct FloatVect3 * | accel, |
| float | dt ) |
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().
| void ahrs_fc_update_gps | ( | struct GpsState * | gps_s | ) |
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().
| void ahrs_fc_update_mag | ( | struct FloatVect3 * | mag, |
| float | dt ) |
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().
| void ahrs_fc_update_mag_2d | ( | struct FloatVect3 * | mag, |
| float | dt ) |
Definition at line 316 of file ahrs_float_cmpl.c.
References ahrs_fc, float_rmat_transp_vmult(), float_rmat_vmult(), float_vect2_normalize(), RATES_ADD_SCALED_VECT, VECT2_COPY, FloatVect2::x, FloatVect3::x, FloatVect2::y, and FloatVect3::y.
Referenced by ahrs_fc_update_mag().
| void ahrs_fc_update_mag_2d_dumb | ( | struct FloatVect3 * | mag | ) |
Definition at line 361 of file ahrs_float_cmpl.c.
References ahrs_fc, float_eulers_of_rmat(), FloatEulers::phi, RATES_ADD_SCALED_VECT, RMAT_ELMT, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
| void ahrs_fc_update_mag_full | ( | struct FloatVect3 * | mag, |
| float | dt ) |
Definition at line 286 of file ahrs_float_cmpl.c.
References ahrs_fc, float_rmat_vmult(), RATES_ADD_SCALED_VECT, and VECT3_CROSS_PRODUCT.
Referenced by ahrs_fc_update_mag().
| struct AhrsFloatCmpl ahrs_fc |
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().