Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_invariant.c File Reference
#include "subsystems/ahrs/ahrs_float_invariant.h"
#include "subsystems/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"
+ Include dependency graph for ahrs_float_invariant.c:

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 ) More...
 
static void invariant_model (float *o, const float *x, const int n, const float *u, const int m)
 Compute dynamic mode. More...
 
void float_quat_vmul_right (struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
 Right multiplication by a quaternion. More...
 
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)
 
void ahrs_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 

Variables

struct AhrsFloatInv ahrs_float_inv
 
static const struct FloatVect3 A = { 0.f, 0.f, -9.81f }
 

Detailed Description

Author
Jean-Philippe Condomines jp.co.nosp@m.ndom.nosp@m.ines@.nosp@m.gmai.nosp@m.l.com

AHRS using invariant filter.

Definition in file ahrs_float_invariant.c.

Macro Definition Documentation

#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(), error_output(), and imu_quality_assessment_periodic().

#define MAG_FROZEN_COUNT   30

Definition at line 237 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_update_mag().

Function Documentation

void ahrs_float_inv_set_body_to_imu_quat ( struct FloatQuat q_b2i)

Definition at line 384 of file ahrs_float_invariant.c.

References ahrs_float_inv, AhrsFloatInv::body_to_imu, AhrsFloatInv::is_aligned, orientationSetQuat_f(), inv_state::quat, and AhrsFloatInv::state.

Referenced by body_to_imu_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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(), ahrs_float_inv, inv_state::bias, AhrsFloatInv::is_aligned, inv_state::quat, and AhrsFloatInv::state.

Referenced by aligner_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_float_invariant_update_accel ( struct FloatVect3 accel)

Definition at line 231 of file ahrs_float_invariant.c.

References inv_measures::accel, ahrs_float_inv, and AhrsFloatInv::meas.

Referenced by accel_cb().

+ Here is the caller graph for this function:

void ahrs_float_invariant_update_mag ( struct FloatVect3 mag)

Definition at line 239 of file ahrs_float_invariant.c.

References ahrs_float_inv, AhrsFloatInv::body_to_imu, float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, inv_measures::mag, MAG_FROZEN_COUNT, AhrsFloatInv::meas, orientationGetRMat_f(), and FloatVect3::x.

Referenced by mag_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 370 of file ahrs_float_invariant.c.

References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by invariant_model().

+ Here is the caller graph for this function:

static void init_invariant_state ( void  )
inlinestatic
static void invariant_model ( float *  o,
const float *  x,
const int  n,
const float *  u,
const int  m 
)
inlinestatic

Variable Documentation

const struct FloatVect3 A = { 0.f, 0.f, -9.81f }
static

Definition at line 98 of file ahrs_float_invariant.c.