Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ins_float_invariant.c File Reference
#include "subsystems/ins/ins_float_invariant.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/ins.h"
#include "subsystems/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 "subsystems/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.
 
#define INS_INV_LB   6.
 
#define INS_INV_MV   8.
 
#define INS_INV_MVZ   15.
 
#define INS_INV_MH   0.2
 
#define INS_INV_NX   0.8
 
#define INS_INV_NXZ   0.5
 
#define INS_INV_NH   1.2
 
#define INS_INV_OV   1.2
 
#define INS_INV_OB   1.
 
#define INS_INV_RV   4.
 
#define INS_INV_RH   8.
 
#define INS_INV_SH   0.01
 
#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 ) 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)
 
static void send_inv_filter (struct transport_tx *trans, struct link_device *dev)
 
void ins_float_invariant_init (void)
 
void ins_reset_local_origin (void)
 INS local origin reset. More...
 
void ins_reset_altitude_ref (void)
 INS altitude reference reset. More...
 
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)
 
void ins_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 

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.

◆ INS_INV_LB

#define INS_INV_LB   6.

Definition at line 89 of file ins_float_invariant.c.

◆ INS_INV_LV

#define INS_INV_LV   2.

Definition at line 85 of file ins_float_invariant.c.

◆ INS_INV_MH

#define INS_INV_MH   0.2

Definition at line 101 of file ins_float_invariant.c.

◆ INS_INV_MV

#define INS_INV_MV   8.

Definition at line 93 of file ins_float_invariant.c.

◆ INS_INV_MVZ

#define INS_INV_MVZ   15.

Definition at line 97 of file ins_float_invariant.c.

◆ INS_INV_NH

#define INS_INV_NH   1.2

Definition at line 113 of file ins_float_invariant.c.

◆ INS_INV_NX

#define INS_INV_NX   0.8

Definition at line 105 of file ins_float_invariant.c.

◆ INS_INV_NXZ

#define INS_INV_NXZ   0.5

Definition at line 109 of file ins_float_invariant.c.

◆ INS_INV_OB

#define INS_INV_OB   1.

Definition at line 121 of file ins_float_invariant.c.

◆ INS_INV_OV

#define INS_INV_OV   1.2

Definition at line 117 of file ins_float_invariant.c.

◆ INS_INV_RH

#define INS_INV_RH   8.

Definition at line 129 of file ins_float_invariant.c.

◆ INS_INV_RV

#define INS_INV_RV   4.

Definition at line 125 of file ins_float_invariant.c.

◆ INS_INV_SH

#define INS_INV_SH   0.01

Definition at line 133 of file ins_float_invariant.c.

◆ MAG_FROZEN_COUNT

#define MAG_FROZEN_COUNT   30

Definition at line 525 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 699 of file ins_float_invariant.c.

References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, v1, v2, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, VECT3_SMUL, and vi.

Referenced by invariant_model().

+ Here is the caller graph for this function:

◆ init_invariant_state()

◆ ins_float_inv_set_body_to_imu_quat()

void ins_float_inv_set_body_to_imu_quat ( struct FloatQuat q_b2i)

Definition at line 713 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, ins_float_inv, InsFloatInv::is_aligned, orientationSetQuat_f(), inv_state::quat, and InsFloatInv::state.

Referenced by body_to_imu_cb().

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

References ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), inv_state::bias, ins_float_inv, InsFloatInv::is_aligned, inv_state::quat, InsFloatInv::state, 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_update_baro()

void ins_float_invariant_update_baro ( float  pressure)

Definition at line 491 of file ins_float_invariant.c.

References alpha, inv_measures::baro_alt, ins_baro_initialized, ins_float_inv, InsFloatInv::meas, 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 527 of file ins_float_invariant.c.

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

Referenced by ins_float_invariant_update_gps(), and mag_cb().

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

◆ ins_reset_altitude_ref()

void ins_reset_altitude_ref ( void  )

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 296 of file ins_float_invariant.c.

References LlaCoor_i::alt, UtmCoor_f::alt, gps, GpsState::hmsl, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, GpsState::lla_pos, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), State::ned_origin_i, state, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and State::utm_origin_f.

+ Here is the call graph for this function:

◆ ins_reset_local_origin()

void ins_reset_local_origin ( void  )

INS local origin reset.

Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.

INS local origin reset.

Definition at line 282 of file ins_float_invariant.c.

References GpsState::ecef_pos, gps, GpsState::hmsl, LtpDef_i::hmsl, ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

+ Here is the call graph for this function:

◆ invariant_model()

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

◆ send_inv_filter()

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

Variable Documentation

◆ A

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

Definition at line 140 of file ins_float_invariant.c.

Referenced by ins_float_invariant_propagate(), and invariant_model().

◆ 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