|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/ahrs/ahrs_float_invariant.h"#include "modules/ahrs/ahrs_int_utils.h"#include "generated/airframe.h"#include "math/pprz_algebra_float.h"#include "math/pprz_algebra_int.h"#include "math/pprz_rk_float.h"Go to the source code of this file.
Macros | |
| #define | AHRS_INV_LX 2. * (0.06 + 0.1) |
| #define | AHRS_INV_LY 2. * (0.06 + 0.06) |
| #define | AHRS_INV_LZ 2. * (0.1 + 0.06) |
| #define | AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1) |
| #define | AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06) |
| #define | AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06) |
| #define | AHRS_INV_N 0.25 |
| #define | AHRS_INV_O 0.5 |
| #define | B ahrs_float_inv.mag_h |
| #define | MAG_FROZEN_COUNT 30 |
Functions | |
| static void | error_output (struct AhrsFloatInv *_ahrs) |
| Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ). | |
| static void | invariant_model (float *o, const float *x, const int n, const float *u, const int m) |
| Compute dynamic mode. | |
| void | float_quat_vmul_right (struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi) |
| Right multiplication by a quaternion. | |
| static void | init_invariant_state (void) |
| void | ahrs_float_invariant_init (void) |
| void | ahrs_float_invariant_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag) |
| void | ahrs_float_invariant_propagate (struct FloatRates *gyro, float dt) |
| void | ahrs_float_invariant_update_accel (struct FloatVect3 *accel) |
| void | ahrs_float_invariant_update_mag (struct FloatVect3 *mag) |
Variables | |
| struct AhrsFloatInv | ahrs_float_inv |
| static const struct FloatVect3 | A = { 0.f, 0.f, -9.81f } |
AHRS using invariant filter.
Definition in file ahrs_float_invariant.c.
| #define AHRS_INV_LX 2. * (0.06 + 0.1) |
Definition at line 64 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_LY 2. * (0.06 + 0.06) |
Definition at line 68 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_LZ 2. * (0.1 + 0.06) |
Definition at line 72 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1) |
Definition at line 76 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06) |
Definition at line 80 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06) |
Definition at line 84 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_N 0.25 |
Definition at line 88 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define AHRS_INV_O 0.5 |
Definition at line 92 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_init().
| #define B ahrs_float_inv.mag_h |
Definition at line 101 of file ahrs_float_invariant.c.
Referenced by AbsDiff(), distance_to_segment(), distance_to_wall(), error_output(), error_output(), imu_quality_assessment_periodic(), ins_float_invariant_init(), ins_mekf_wind_update_incidence(), oneloop_nB_calc_nB_states(), predict_flow_vectors(), timeval_diff(), and wls_alloc().
| #define MAG_FROZEN_COUNT 30 |
Definition at line 234 of file ahrs_float_invariant.c.
Referenced by ahrs_float_invariant_update_mag(), and ins_float_invariant_update_mag().
| void ahrs_float_invariant_align | ( | struct FloatRates * | lp_gyro, |
| struct FloatVect3 * | lp_accel, | ||
| struct FloatVect3 * | lp_mag ) |
Definition at line 156 of file ahrs_float_invariant.c.
References ahrs_float_get_quat_from_accel_mag(), and ahrs_float_inv.
Referenced by aligner_cb().
| void ahrs_float_invariant_init | ( | void | ) |
Definition at line 131 of file ahrs_float_invariant.c.
References ahrs_float_inv, AHRS_INV_LX, AHRS_INV_LY, AHRS_INV_LZ, AHRS_INV_MX, AHRS_INV_MY, AHRS_INV_MZ, AHRS_INV_N, AHRS_INV_O, and init_invariant_state().
Referenced by ahrs_finv_wrapper_init().
| void ahrs_float_invariant_propagate | ( | struct FloatRates * | gyro, |
| float | dt ) |
Definition at line 170 of file ahrs_float_invariant.c.
References ahrs_float_inv, DefaultChannel, DefaultDevice, error_output(), float_eulers_of_quat(), float_quat_normalize(), foo, init_invariant_state(), INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), FloatEulers::phi, FloatEulers::psi, RATES_COPY, runge_kutta_4_float(), and FloatEulers::theta.
Referenced by gyro_cb().
| void ahrs_float_invariant_update_accel | ( | struct FloatVect3 * | accel | ) |
Definition at line 228 of file ahrs_float_invariant.c.
References ahrs_float_inv.
Referenced by accel_cb().
| void ahrs_float_invariant_update_mag | ( | struct FloatVect3 * | mag | ) |
Definition at line 236 of file ahrs_float_invariant.c.
References ahrs_float_inv, FLOAT_VECT3_ZERO, MAG_FROZEN_COUNT, VECT3_COPY, and FloatVect3::x.
Referenced by mag_cb().
|
inlinestatic |
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ).
Definition at line 313 of file ahrs_float_invariant.c.
References A, inv_measures::accel, inv_state::as, B, AhrsFloatInv::corr, inv_state::cs, float_quat_invert(), float_quat_vmult(), AhrsFloatInv::gains, inv_correction_gains::LE, inv_gains::lx, inv_gains::ly, inv_gains::lz, inv_measures::mag, inv_correction_gains::ME, AhrsFloatInv::meas, inv_gains::mx, inv_gains::my, inv_gains::mz, inv_gains::n, inv_correction_gains::NE, inv_gains::o, inv_correction_gains::OE, inv_state::quat, AhrsFloatInv::state, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ahrs_float_invariant_propagate().
| void float_quat_vmul_right | ( | struct FloatQuat * | mright, |
| const struct FloatQuat * | q, | ||
| struct FloatVect3 * | vi ) |
Right multiplication by a quaternion.
vi * q
Definition at line 365 of file ahrs_float_invariant.c.
References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, v1, v2, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, and VECT3_SMUL.
Referenced by invariant_model().
|
inlinestatic |
Definition at line 117 of file ahrs_float_invariant.c.
References ahrs_float_inv, float_quat_identity(), FLOAT_RATES_ZERO, and FLOAT_VECT3_ZERO.
Referenced by ahrs_float_invariant_init(), and ahrs_float_invariant_propagate().
|
inlinestatic |
Compute dynamic mode.
x_dot = evolution_model + (gain_matrix * error)
Definition at line 261 of file ahrs_float_invariant.c.
References ahrs_float_inv, inv_state::as, inv_state::bias, inv_state::cs, float_quat_derivative(), FLOAT_QUAT_NORM2, float_quat_vmul_right(), float_quat_vmult(), float_vect_zero(), inv_state::quat, QUAT_ADD, QUAT_SMUL, inv_command::rates, RATES_ASSIGN, RATES_DIFF, s, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ahrs_float_invariant_propagate(), and ins_float_invariant_propagate().
|
static |
Definition at line 98 of file ahrs_float_invariant.c.
| struct AhrsFloatInv ahrs_float_inv |
Definition at line 95 of file ahrs_float_invariant.c.
Referenced by accel_cb(), ahrs_float_invariant_align(), ahrs_float_invariant_init(), ahrs_float_invariant_propagate(), ahrs_float_invariant_update_accel(), ahrs_float_invariant_update_mag(), aligner_cb(), compute_body_orientation_and_rates(), geo_mag_cb(), gyro_cb(), init_invariant_state(), invariant_model(), mag_cb(), send_att(), send_filter_status(), and send_geo_mag().