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
ins_int.c File Reference

INS for rotorcrafts combining vertical and horizontal filters. More...

#include "subsystems/ins/ins_int.h"
#include "subsystems/abi.h"
#include "subsystems/imu.h"
#include "subsystems/gps.h"
#include "generated/airframe.h"
#include "subsystems/ins/vf_float.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_isa.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for ins_int.c:

Go to the source code of this file.

Macros

#define INS_VFF_R_GPS   2.0
 
#define INS_VFF_VZ_R_GPS   2.0
 
#define INS_MAX_PROPAGATION_STEPS   200
 maximum number of propagation steps without any updates in between More...
 
#define USE_INS_NAV_INIT   TRUE
 
#define INS_INT_BARO_ID   ABI_BROADCAST
 default barometer to use in INS More...
 
#define INS_INT_IMU_ID   ABI_BROADCAST
 ABI binding for IMU data. More...
 
#define INS_INT_GPS_ID   GPS_MULTI_ID
 
#define INS_INT_VEL_ID   ABI_BROADCAST
 ABI binding for VELOCITY_ESTIMATE. More...
 

Functions

static void baro_cb (uint8_t sender_id, float pressure)
 
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
 
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
 
static void vel_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise)
 
static void send_ins (struct transport_tx *trans, struct link_device *dev)
 
static void send_ins_z (struct transport_tx *trans, struct link_device *dev)
 
static void send_ins_ref (struct transport_tx *trans, struct link_device *dev)
 
static void ins_ned_to_state (void)
 copy position and speed to state interface More...
 
static void ins_update_from_vff (void)
 update ins state from vertical filter More...
 
void ins_int_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_int_propagate (struct Int32Vect3 *accel, float dt)
 
void ins_int_update_gps (struct GpsState *gps_s)
 

Variables

abi_event baro_ev
 
static abi_event accel_ev
 
static abi_event gps_ev
 
static abi_event vel_est_ev
 
struct InsInt ins_int
 global INS state More...
 

Detailed Description

INS for rotorcrafts combining vertical and horizontal filters.

Definition in file ins_int.c.

Macro Definition Documentation

#define INS_INT_BARO_ID   ABI_BROADCAST

default barometer to use in INS

Definition at line 117 of file ins_int.c.

Referenced by ins_int_init().

#define INS_INT_GPS_ID   GPS_MULTI_ID

Definition at line 134 of file ins_int.c.

Referenced by ins_int_init().

#define INS_INT_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used accel ABI messages.

Definition at line 128 of file ins_int.c.

Referenced by ins_int_init().

#define INS_INT_VEL_ID   ABI_BROADCAST

ABI binding for VELOCITY_ESTIMATE.

Usually this is coming from opticflow.

Definition at line 143 of file ins_int.c.

Referenced by ins_int_init().

#define INS_MAX_PROPAGATION_STEPS   200

maximum number of propagation steps without any updates in between

Definition at line 104 of file ins_int.c.

Referenced by ins_int_init(), and ins_int_propagate().

#define INS_VFF_R_GPS   2.0

Definition at line 94 of file ins_int.c.

Referenced by ins_int_update_gps().

#define INS_VFF_VZ_R_GPS   2.0

Definition at line 98 of file ins_int.c.

Referenced by ins_int_update_gps().

#define USE_INS_NAV_INIT   TRUE

Definition at line 108 of file ins_int.c.

Function Documentation

static void accel_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 accel 
)
static

Definition at line 502 of file ins_int.c.

References ins_int_propagate(), and PRINT_CONFIG_MSG().

Referenced by ins_int_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void baro_cb ( uint8_t  sender_id,
float  pressure 
)
static
static void gps_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct GpsState gps_s 
)
static

Definition at line 516 of file ins_int.c.

References ins_int_update_gps().

Referenced by ins_int_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void ins_ned_to_state ( void  )
static

copy position and speed to state interface

Definition at line 467 of file ins_int.c.

References ins_int, InsInt::ltp_accel, InsInt::ltp_pos, InsInt::ltp_speed, nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_i(), stateSetPositionNed_i(), and stateSetSpeedNed_i().

Referenced by baro_cb(), ins_int_propagate(), ins_int_update_gps(), and vel_est_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 257 of file ins_int.c.

References LlaCoor_i::alt, gps, GpsState::hmsl, LtpDef_i::hmsl, ins_int, LlaCoor_i::lat, LtpDef_i::lla, GpsState::lla_pos, LlaCoor_i::lon, InsInt::ltp_def, ltp_def_from_lla_i(), State::ned_origin_i, state, stateSetLocalOrigin_i(), and InsInt::vf_reset.

+ 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 235 of file ins_int.c.

References LlaCoor_i::alt, GpsState::ecef_pos, gps, GpsFixValid, InsInt::hf_realign, GpsState::hmsl, LtpDef_i::hmsl, ins_int, LtpDef_i::lla, GpsState::lla_pos, InsInt::ltp_def, ltp_def_from_ecef_i(), InsInt::ltp_initialized, stateSetLocalOrigin_i(), and InsInt::vf_reset.

Referenced by ins_int_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void ins_update_from_vff ( void  )
static

update ins state from vertical filter

Definition at line 481 of file ins_int.c.

References ACCEL_BFP_OF_REAL, ins_int, InsInt::ltp_accel, InsInt::ltp_pos, InsInt::ltp_speed, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, vff, VffExtended::z, NedCoor_i::z, VffExtended::zdot, and VffExtended::zdotdot.

Referenced by baro_cb(), and ins_int_propagate().

+ Here is the caller graph for this function:

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

Definition at line 153 of file ins_int.c.

References ins_int, InsInt::ltp_accel, InsInt::ltp_pos, InsInt::ltp_speed, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_int_init().

+ Here is the caller graph for this function:

static void send_ins_ref ( struct transport_tx *  trans,
struct link_device *  dev 
)
static
static void send_ins_z ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 161 of file ins_int.c.

References InsInt::baro_z, ins_int, InsInt::ltp_accel, InsInt::ltp_pos, InsInt::ltp_speed, and NedCoor_i::z.

Referenced by ins_int_init().

+ Here is the caller graph for this function:

static void vel_est_cb ( uint8_t  sender_id,
uint32_t  stamp,
float  x,
float  y,
float  z,
float  noise 
)
static

Definition at line 523 of file ins_int.c.

References b2_hff_update_vel(), float_quat_vmult(), ins_int, ins_ned_to_state(), InsInt::ltp_pos, InsInt::ltp_speed, POS_BFP_OF_REAL, InsInt::propagation_cnt, QUAT_INVERT, SPEED_BFP_OF_REAL, stateGetNedToBodyQuat_f(), FloatVect3::x, NedCoor_i::x, FloatVect3::y, and NedCoor_i::y.

Referenced by ins_int_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev
static

Definition at line 130 of file ins_int.c.

abi_event baro_ev

Definition at line 121 of file ins_int.c.

abi_event gps_ev
static

Definition at line 136 of file ins_int.c.

abi_event vel_est_ev
static

Definition at line 145 of file ins_int.c.