Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_float_invariant.c File Reference
#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"
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 ).
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 }

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

◆ AHRS_INV_LX

#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().

◆ AHRS_INV_LY

#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().

◆ AHRS_INV_LZ

#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().

◆ AHRS_INV_MX

#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().

◆ AHRS_INV_MY

#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().

◆ AHRS_INV_MZ

#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().

◆ AHRS_INV_N

#define AHRS_INV_N   0.25

Definition at line 88 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

◆ AHRS_INV_O

#define AHRS_INV_O   0.5

Definition at line 92 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

◆ B

◆ MAG_FROZEN_COUNT

#define MAG_FROZEN_COUNT   30

Function Documentation

◆ ahrs_float_invariant_align()

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().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ahrs_float_invariant_init()

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().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ahrs_float_invariant_propagate()

void ahrs_float_invariant_propagate ( struct FloatRates * gyro,
float dt )

◆ ahrs_float_invariant_update_accel()

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().

Here is the caller graph for this function:

◆ ahrs_float_invariant_update_mag()

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().

Here is the caller graph for this function:

◆ error_output()

◆ float_quat_vmul_right()

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().

Here is the caller graph for this function:

◆ init_invariant_state()

void init_invariant_state ( void )
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().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ invariant_model()

void invariant_model ( float * o,
const float * x,
const int n,
const float * u,
const int m )
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().

Here is the call graph for this function:
Here is the caller graph for this function:

Variable Documentation

◆ A

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

Definition at line 98 of file ahrs_float_invariant.c.

◆ ahrs_float_inv