Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_float_invariant.c File Reference
#include "modules/ins/ins_float_invariant.h"
#include "modules/ahrs/ahrs_int_utils.h"
#include "modules/ins/ins.h"
#include "modules/gps/gps.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_rk_float.h"
#include "math/pprz_isa.h"
#include "state.h"
#include "modules/datalink/telemetry.h"
Include dependency graph for ins_float_invariant.c:

Go to the source code of this file.

Macros

#define INS_INV_LV   2.f
#define INS_INV_LB   6.f
#define INS_INV_MV   8.f
#define INS_INV_MVZ   15.f
#define INS_INV_MH   0.2f
#define INS_INV_NX   0.8f
#define INS_INV_NXZ   0.5f
#define INS_INV_NH   1.2f
#define INS_INV_OV   1.2f
#define INS_INV_OB   1.f
#define INS_INV_RV   4.f
#define INS_INV_RH   8.f
#define INS_INV_SH   0.01f
#define B   ins_float_inv.mag_h
#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED   5.f
#define MAG_FROZEN_COUNT   30

Functions

static void error_output (struct InsFloatInv *_ins)
 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)
static void send_inv_filter (struct transport_tx *trans, struct link_device *dev)
void ins_float_invariant_init (void)
void ins_float_invariant_reset_ref (void)
void ins_float_invariant_reset_vertical_ref (void)
void ins_float_invariant_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ins_float_invariant_propagate (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
void ins_float_invariant_update_gps (struct GpsState *gps_s)
void ins_float_invariant_update_baro (float pressure)
void ins_float_invariant_update_mag (struct FloatVect3 *mag)

Variables

struct InsFloatInv ins_float_inv
static const struct FloatVect3 A = { 0.f, 0.f, 9.81f }
bool ins_baro_initialized
bool ins_gps_fix_once

Detailed Description

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

INS using invariant filter.

Definition in file ins_float_invariant.c.

Macro Definition Documentation

◆ B

#define B   ins_float_inv.mag_h

Definition at line 144 of file ins_float_invariant.c.

◆ INS_INV_HEADING_UPDATE_GPS_MIN_SPEED

#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED   5.f

Definition at line 154 of file ins_float_invariant.c.

Referenced by ins_float_invariant_update_gps().

◆ INS_INV_LB

#define INS_INV_LB   6.f

Definition at line 89 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_LV

#define INS_INV_LV   2.f

Definition at line 85 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_MH

#define INS_INV_MH   0.2f

Definition at line 101 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_MV

#define INS_INV_MV   8.f

Definition at line 93 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_MVZ

#define INS_INV_MVZ   15.f

Definition at line 97 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_NH

#define INS_INV_NH   1.2f

Definition at line 113 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_NX

#define INS_INV_NX   0.8f

Definition at line 105 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_NXZ

#define INS_INV_NXZ   0.5f

Definition at line 109 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_OB

#define INS_INV_OB   1.f

Definition at line 121 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_OV

#define INS_INV_OV   1.2f

Definition at line 117 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_RH

#define INS_INV_RH   8.f

Definition at line 129 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_RV

#define INS_INV_RV   4.f

Definition at line 125 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ INS_INV_SH

#define INS_INV_SH   0.01f

Definition at line 133 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

◆ MAG_FROZEN_COUNT

#define MAG_FROZEN_COUNT   30

Definition at line 566 of file ins_float_invariant.c.

Function Documentation

◆ 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 739 of file ins_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 172 of file ins_float_invariant.c.

References float_quat_identity(), FLOAT_RATES_ZERO, FLOAT_VECT3_ZERO, ins_baro_initialized, ins_float_inv, and ins_gps_fix_once.

Referenced by ins_float_invariant_init(), and ins_float_invariant_propagate().

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

◆ ins_float_invariant_align()

void ins_float_invariant_align ( struct FloatRates * lp_gyro,
struct FloatVect3 * lp_accel,
struct FloatVect3 * lp_mag )

Definition at line 323 of file ins_float_invariant.c.

References ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ins_float_inv, and stateSetNedToBodyQuat_f().

Referenced by aligner_cb().

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

◆ ins_float_invariant_init()

◆ ins_float_invariant_propagate()

◆ ins_float_invariant_reset_ref()

void ins_float_invariant_reset_ref ( void )

Definition at line 281 of file ins_float_invariant.c.

References LlaCoor_i::alt, ecef_int_from_gps(), FLOAT_VECT3_ZERO, gps, ins_float_inv, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

Referenced by reset_cb().

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

◆ ins_float_invariant_reset_vertical_ref()

void ins_float_invariant_reset_vertical_ref ( void )

Definition at line 301 of file ins_float_invariant.c.

References UtmCoor_f::alt, gps, ins_float_inv, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), and stateSetLocalUtmOrigin_f().

Referenced by reset_cb().

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

◆ ins_float_invariant_update_baro()

void ins_float_invariant_update_baro ( float pressure)

Definition at line 532 of file ins_float_invariant.c.

References alpha, ins_baro_initialized, ins_float_inv, pprz_isa_height_of_pressure(), and PPRZ_ISA_SEA_LEVEL_PRESSURE.

Referenced by baro_cb().

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

◆ ins_float_invariant_update_gps()

◆ ins_float_invariant_update_mag()

void ins_float_invariant_update_mag ( struct FloatVect3 * mag)

Definition at line 568 of file ins_float_invariant.c.

References FLOAT_VECT3_ZERO, ins_float_inv, MAG_FROZEN_COUNT, VECT3_COPY, and FloatVect3::x.

Referenced by ins_float_invariant_update_gps(), and mag_cb().

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

◆ send_inv_filter()

void send_inv_filter ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 193 of file ins_float_invariant.c.

References dev, float_eulers_of_quat(), ins_float_inv, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ins_float_invariant_init(), ins_float_invariant_propagate(), and ins_mekf_wind_wrapper_init().

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 140 of file ins_float_invariant.c.

◆ ins_baro_initialized

bool ins_baro_initialized

Definition at line 147 of file ins_float_invariant.c.

Referenced by init_invariant_state(), and ins_float_invariant_update_baro().

◆ ins_float_inv

◆ ins_gps_fix_once

bool ins_gps_fix_once