Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_float_invariant.c File Reference
#include "subsystems/ins/ins_float_invariant.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs.h"
#include "subsystems/ins.h"
#include "subsystems/gps.h"
#include "subsystems/imu.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 "subsystems/abi.h"
#include "state.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.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 INS_BARO_ID   ABI_BROADCAST
 
#define MAG_FROZEN_COUNT   30
 

Functions

static void baro_cb (uint8_t sender_id, const float *pressure)
 
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...
 
static void init_invariant_state (void)
 
void ins_init ()
 INS initialization. More...
 
void ins_reset_local_origin (void)
 INS local origin reset. More...
 
void ins_reset_altitude_ref (void)
 INS altitude reference reset. More...
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
void ahrs_propagate (void)
 Propagation. More...
 
void ahrs_update_gps (void)
 Update AHRS state with GPS measurements. More...
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 

Variables

struct InsFloatInv ins_impl
 global INS state More...
 
static const float dt = (1./ ((float)AHRS_PROPAGATE_FREQUENCY))
 
static const struct FloatVect3 A = { 0.f, 0.f, 9.81f }
 
static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) }
 
bool_t ins_baro_initialized
 
abi_event baro_ev
 
bool_t 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.

Only for fixedwing currenctly

Definition in file ins_float_invariant.c.

Macro Definition Documentation

#define INS_BARO_ID   ABI_BROADCAST

Definition at line 192 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_LB   6.

Definition at line 113 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_LV   2.

Definition at line 109 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_MH   0.2

Definition at line 125 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_MV   8.

Definition at line 117 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_MVZ   15.

Definition at line 121 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_NH   1.2

Definition at line 137 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_NX   0.8

Definition at line 129 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_NXZ   0.5

Definition at line 133 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_OB   1.

Definition at line 145 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_OV   1.2

Definition at line 141 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_RH   8.

Definition at line 153 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_RV   4.

Definition at line 149 of file ins_float_invariant.c.

Referenced by ins_init().

#define INS_INV_SH   0.01

Definition at line 157 of file ins_float_invariant.c.

Referenced by ins_init().

#define MAG_FROZEN_COUNT   30

Definition at line 542 of file ins_float_invariant.c.

Referenced by ahrs_update_mag().

Function Documentation

void ahrs_align ( void  )

Aligns the AHRS.

Called after ahrs_aligner has run to set initial attitude and biases. Must set the ahrs status to AHRS_RUNNING. Needs to be implemented by each AHRS algorithm.

Definition at line 331 of file ins_float_invariant.c.

References ahrs, ahrs_aligner, ahrs_float_get_quat_from_accel_mag(), AHRS_RUNNING, inv_state::bias, ins, ins_impl, INS_RUNNING, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, inv_state::quat, RATES_COPY, RATES_FLOAT_OF_BFP, InsFloatInv::state, Ahrs::status, and Ins::status.

+ Here is the call graph for this function:

void ahrs_init ( void  )

AHRS initialization.

Called at startup. Needs to be implemented by each AHRS algorithm.

Definition at line 327 of file ins_float_invariant.c.

References ahrs, AHRS_UNINIT, CHIMU_BROADCAST, CHIMU_Checksum(), CHIMU_Init(), CHIMU_STX, ins_pitch_neutral, INS_PITCH_NEUTRAL_DEFAULT, ins_roll_neutral, INS_ROLL_NEUTRAL_DEFAULT, InsSend, InsSend1, MSG09_ESTIMATOR, MSG10_UARTSETTINGS, new_ins_attitude, and Ahrs::status.

+ Here is the call graph for this function:

void ahrs_propagate ( void  )

Propagation.

Usually integrates the gyro rates to angles. Reads the global imu data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 346 of file ins_float_invariant.c.

References inv_command::accel, Imu::accel, ACCELS_FLOAT_OF_BFP, ahrs, AHRS_UNINIT, inv_state::as, inv_measures::baro_alt, inv_state::bias, Imu::body_to_imu, InsFloatInv::cmd, DefaultChannel, DefaultDevice, error_output(), FALSE, FLOAT_EULERS_OF_QUAT, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_RMAT_B2N, FLOAT_VECT3_ADD, FLOAT_VECT3_SMUL, Imu::gyro, inv_state::hb, imu, init_invariant_state(), ins, ins_impl, ins_pitch_neutral, ins_roll_neutral, INS_UNINIT, INT32_RMAT_TRANSP_RATEMULT, INT32_RMAT_TRANSP_VMULT, INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), inv_measures::mag, InsFloatInv::meas, orientationGetRMat_i(), FloatRates::p, FloatEulers::phi, inv_state::pos, inv_measures::pos_gps, FloatEulers::psi, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, inv_command::rates, RATES_DIFF, RATES_FLOAT_OF_BFP, InsFloatInv::reset, runge_kutta_4_float(), inv_state::speed, inv_measures::speed_gps, InsFloatInv::state, stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyEulers_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), Ahrs::status, Ins::status, FloatEulers::theta, TRUE, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

+ Here is the call graph for this function:

void ahrs_update_accel ( void  )

Update AHRS state with accerleration measurements.

Reads the global imu data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 538 of file ins_float_invariant.c.

void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Does nothing if not implemented by specific AHRS algorithm.

Definition at line 544 of file ins_float_invariant.c.

References ahrs_update_mag_2d(), ahrs_update_mag_full(), Imu::body_to_imu, FLOAT_VECT3_ZERO, imu, ins_impl, INT32_RMAT_TRANSP_VMULT, inv_measures::mag, Imu::mag, MAG_FROZEN_COUNT, MAGS_FLOAT_OF_BFP, InsFloatInv::meas, orientationGetRMat_i(), and Int32Vect3::x.

+ Here is the call graph for this function:

static void baro_cb ( uint8_t  sender_id,
const float *  pressure 
)
static

Definition at line 505 of file ins_float_invariant.c.

References inv_measures::baro_alt, ins_impl, InsFloatInv::meas, pprz_isa_height_of_pressure(), and TRUE.

Referenced by ins_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void init_invariant_state ( void  )
inlinestatic
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 309 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_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:

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

References UtmCoor_f::alt, UtmCoor_f::east, UtmCoor_i::east, GpsState::ecef_pos, gps, GpsState::hmsl, LtpDef_i::hmsl, LLA_FLOAT_OF_BFP, GpsState::lla_pos, LlaCoor_i::lon, ltp_def_from_ecef_i(), UtmCoor_f::north, UtmCoor_i::north, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), utm_of_lla_f(), GpsState::utm_pos, UtmCoor_f::zone, and UtmCoor_i::zone.

+ Here is the call graph for this function:

Variable Documentation

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

Definition at line 180 of file ins_float_invariant.c.

const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) }
static

Definition at line 183 of file ins_float_invariant.c.

Referenced by imu_quality_assessment_periodic().

abi_event baro_ev

Definition at line 196 of file ins_float_invariant.c.

Referenced by ins_init().

const float dt = (1./ ((float)AHRS_PROPAGATE_FREQUENCY))
static
bool_t ins_baro_initialized

Definition at line 186 of file ins_float_invariant.c.

bool_t ins_gps_fix_once

Definition at line 200 of file ins_float_invariant.c.

Referenced by ahrs_update_gps(), and error_output().