ins_int.c File Reference

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

#include "modules/ins/ins_int.h"
#include "modules/core/abi.h"
#include "modules/imu/imu.h"
#include "modules/gps/gps.h"
#include "generated/airframe.h"
#include "modules/ins/vf_float.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_isa.h"
#include "math/pprz_stat.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for ins_int.c:

Go to the source code of this file.

Macros
#define	VFF_R_AGL   0.2
#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
#define	USE_INS_NAV_INIT   TRUE
#define	INS_BARO_MAX_INIT_VAR   1.f
	default barometer to use in INS
#define	INS_INT_BARO_ID   ABI_BROADCAST
#define	INS_INT_IMU_ID   ABI_BROADCAST
	ABI binding for IMU data.
#define	INS_INT_GPS_ID   GPS_MULTI_ID
#define	INS_INT_VEL_ID   ABI_BROADCAST
	ABI binding for VELOCITY_ESTIMATE.
#define	INS_INT_POS_ID   ABI_BROADCAST
#define	INS_INT_AGL_ID   ABI_BROADCAST
	ABI binding for AGL.
#define	press_hist_len   10

Functions
static void	baro_cb (uint8_t sender_id, uint32_t stamp, 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_x, float noise_y, float noise_z)
static void	pos_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
static void	agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
	agl_cb This callback handles all estimates of the height of the vehicle above the ground under it This is only used with the extended version of the vertical float filter
static void	reset_cb (uint8_t sender_id, uint8_t flag)
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
static void	ins_update_from_vff (void)
	update ins state from vertical filter
void	ins_int_init (void)
static void	reset_ref (void)
static void	reset_vertical_ref (void)
static void	reset_vertical_pos (void)
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
static abi_event	pos_est_ev
static abi_event	agl_ev
	The agl ABI event.
static abi_event	reset_ev
struct InsInt	ins_int
	global INS state

Detailed Description

INS for rotorcrafts combining vertical and horizontal filters.

Definition in file ins_int.c.

Macro Definition Documentation

INS_BARO_MAX_INIT_VAR

#define INS_BARO_MAX_INIT_VAR   1.f

default barometer to use in INS

Definition at line 105 of file ins_int.c.

INS_INT_AGL_ID

#define INS_INT_AGL_ID   ABI_BROADCAST

ABI binding for AGL.

Usually this is comes from sonar or gps.

Definition at line 160 of file ins_int.c.

INS_INT_BARO_ID

#define INS_INT_BARO_ID   ABI_BROADCAST

Definition at line 110 of file ins_int.c.

INS_INT_GPS_ID

#define INS_INT_GPS_ID   GPS_MULTI_ID

Definition at line 128 of file ins_int.c.

INS_INT_IMU_ID

#define INS_INT_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used accel ABI messages.

Definition at line 121 of file ins_int.c.

INS_INT_POS_ID

#define INS_INT_POS_ID   ABI_BROADCAST

Definition at line 147 of file ins_int.c.

INS_INT_VEL_ID

#define INS_INT_VEL_ID   ABI_BROADCAST

ABI binding for VELOCITY_ESTIMATE.

Usually this is coming from opticflow.

Definition at line 138 of file ins_int.c.

INS_MAX_PROPAGATION_STEPS

#define INS_MAX_PROPAGATION_STEPS   200

maximum number of propagation steps without any updates in between

Definition at line 96 of file ins_int.c.

INS_VFF_R_GPS

#define INS_VFF_R_GPS   2.0

Definition at line 86 of file ins_int.c.

INS_VFF_VZ_R_GPS

#define INS_VFF_VZ_R_GPS   2.0

Definition at line 90 of file ins_int.c.

press_hist_len

#define press_hist_len   10

USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

Definition at line 100 of file ins_int.c.

VFF_R_AGL

#define VFF_R_AGL   0.2

Definition at line 59 of file ins_int.c.

Function Documentation

accel_cb()

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

static

Definition at line 577 of file ins_int.c.

References foo, 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:

agl_cb()

static void agl_cb ( uint8_t  sender_id, uint32_t  stamp, float  distance  )

static

agl_cb This callback handles all estimates of the height of the vehicle above the ground under it This is only used with the extended version of the vertical float filter

Definition at line 538 of file ins_int.c.

Referenced by ins_int_init().

Here is the caller graph for this function:

baro_cb()

static void baro_cb ( uint8_t  sender_id, uint32_t  stamp, float  pressure  )

static

Definition at line 363 of file ins_int.c.

References InsInt::baro_initialized, InsInt::baro_z, foo, idx, INS_BARO_MAX_INIT_VAR, ins_int, ins_update_from_vff(), InsInt::ltp_initialized, pprz_isa_height_of_pressure(), press_hist_len, InsInt::propagation_cnt, InsInt::qfe, stateGetPositionEnu_f(), variance_f(), InsInt::vf_reset, vff_realign(), vff_update(), and vff_update_baro().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

gps_cb()

static void gps_cb ( uint8_t  sender_id, uint32_t stamp  , struct GpsState *  gps_s  )

static

Definition at line 591 of file ins_int.c.

References foo, and 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:

ins_int_init()

void ins_int_init

(

void

)

Definition at line 207 of file ins_int.c.

References ABI_BROADCAST, accel_cb(), accel_ev, agl_cb(), agl_ev, baro_cb(), baro_ev, InsInt::baro_initialized, DefaultPeriodic, foo, gps_cb(), gps_ev, InsInt::hf_realign, hff_init(), ins_init_origin_i_from_flightplan(), ins_int, INS_INT_AGL_ID, INS_INT_BARO_ID, INS_INT_GPS_ID, INS_INT_IMU_ID, INS_INT_POS_ID, INS_INT_VEL_ID, INS_MAX_PROPAGATION_STEPS, INT32_VECT3_ZERO, InsInt::ltp_accel, InsInt::ltp_def, InsInt::ltp_initialized, InsInt::ltp_pos, InsInt::ltp_speed, pos_est_cb(), pos_est_ev, InsInt::propagation_cnt, register_periodic_telemetry(), reset_cb(), reset_ev, send_ins(), send_ins_ref(), send_ins_z(), vel_est_cb(), vel_est_ev, InsInt::vf_reset, and vff_init_zero().

Here is the call graph for this function:

ins_int_propagate()

void ins_int_propagate	(	struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 319 of file ins_int.c.

References ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, foo, hff_propagate(), ins_int, INS_MAX_PROPAGATION_STEPS, ins_ned_to_state(), ins_update_from_vff(), int32_rmat_transp_vmult(), InsInt::ltp_accel, InsInt::propagation_cnt, stateGetNedToBodyRMat_i(), stateSetAccelBody_i(), vff_propagate(), NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_int_update_gps()

void ins_int_update_gps

(

struct GpsState *

gps_s

)

Todo:

maybe use gps_s->ned_vel directly??

Definition at line 422 of file ins_int.c.

References ecef_int_from_gps(), ecef_vel_int_from_gps(), foo, GPS_FIX_3D, InsInt::hf_realign, hff_realign(), hff_update_gps(), ins_int, ins_ned_to_state(), INS_VFF_R_GPS, INS_VFF_VZ_R_GPS, INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, int32_quat_vmult(), INT32_SPEED_OF_CM_S_DEN, INT32_SPEED_OF_CM_S_NUM, INT32_VECT2_SCALE_2, InsInt::ltp_def, InsInt::ltp_initialized, InsInt::ltp_pos, InsInt::ltp_speed, ned_of_ecef_point_i(), ned_of_ecef_vect_i(), InsInt::propagation_cnt, QUAT_INVERT, reset_ref(), stateGetNedToBodyQuat_i(), VECT2_ASSIGN, VECT2_SDIV, VECT3_SUB, vff_update_vz_conf(), vff_update_z_conf(), and Int32Vect3::x.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_ned_to_state()

static void ins_ned_to_state ( void  )

static

copy position and speed to state interface

Definition at line 542 of file ins_int.c.

References foo, 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 ins_int_propagate(), ins_int_update_gps(), pos_est_cb(), and vel_est_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_update_from_vff()

static void ins_update_from_vff ( void  )

static

update ins state from vertical filter

Definition at line 556 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, NedCoor_i::z, VffExtended::z, VffExtended::zdot, and VffExtended::zdotdot.

Referenced by baro_cb(), and ins_int_propagate().

Here is the caller graph for this function:

pos_est_cb()

static void pos_est_cb ( uint8_t  sender_id, uint32_t  stamp, float  x, float  y, float  z, float  noise_x, float  noise_y, float  noise_z  )

static

Definition at line 663 of file ins_int.c.

References foo, hff_update_pos(), ins_int, ins_ned_to_state(), InsInt::ltp_pos, POS_BFP_OF_REAL, InsInt::propagation_cnt, vff_update_z_conf(), FloatVect2::x, NedCoor_i::x, FloatVect2::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:

reset_cb()

static void reset_cb ( uint8_t sender_id  UNUSED, uint8_t  flag  )

static

Definition at line 301 of file ins_int.c.

References foo, INS_RESET_REF, INS_RESET_VERTICAL_POS, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_pos(), and reset_vertical_ref().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_ref()

static void reset_ref ( void  )

static

Definition at line 254 of file ins_int.c.

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

Referenced by ins_int_update_gps(), and reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_vertical_pos()

static void reset_vertical_pos ( void  )

static

Definition at line 296 of file ins_int.c.

References ins_int, and InsInt::vf_reset.

Referenced by reset_cb().

Here is the caller graph for this function:

reset_vertical_ref()

static void reset_vertical_ref ( void  )

static

Definition at line 278 of file ins_int.c.

References LlaCoor_i::alt, foo, gps, GpsFixValid, LtpDef_i::hmsl, GpsState::hmsl, ins_int, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, InsInt::ltp_def, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), stateSetLocalOrigin_i(), and InsInt::vf_reset.

Referenced by reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

send_ins()

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

static

Definition at line 175 of file ins_int.c.

References dev, foo, 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:

send_ins_ref()

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

static

Definition at line 189 of file ins_int.c.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, foo, LtpDef_i::hmsl, ins_int, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsInt::ltp_def, InsInt::ltp_initialized, InsInt::qfe, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_int_init().

Here is the caller graph for this function:

send_ins_z()

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

static

Definition at line 183 of file ins_int.c.

References InsInt::baro_z, dev, foo, 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:

vel_est_cb()

static void vel_est_cb ( uint8_t  sender_id, uint32_t  stamp, float  x, float  y, float  z, float  noise_x, float  noise_y, float  noise_z  )

static

Definition at line 601 of file ins_int.c.

References float_quat_vmult(), foo, hff_update_vel(), 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(), vff_update_vz_conf(), FloatVect3::x, NedCoor_i::x, FloatVect3::y, NedCoor_i::y, and FloatVect3::z.

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

accel_ev

abi_event accel_ev

static

Definition at line 124 of file ins_int.c.

Referenced by ins_int_init().

agl_ev

abi_event agl_ev

static

The agl ABI event.

Definition at line 163 of file ins_int.c.

Referenced by ins_int_init().

baro_ev

abi_event baro_ev

Definition at line 114 of file ins_int.c.

Referenced by ins_int_init().

gps_ev

abi_event gps_ev

static

Definition at line 131 of file ins_int.c.

Referenced by ins_int_init().

ins_int

struct InsInt ins_int

global INS state

Definition at line 170 of file ins_int.c.

Referenced by baro_cb(), ins_int_init(), ins_int_propagate(), ins_int_update_gps(), ins_ned_to_state(), ins_update_from_vff(), pos_est_cb(), reset_ref(), reset_vertical_pos(), reset_vertical_ref(), send_ins(), send_ins_ref(), send_ins_z(), and vel_est_cb().

pos_est_ev

abi_event pos_est_ev

static

Definition at line 150 of file ins_int.c.

Referenced by ins_int_init().

reset_ev

abi_event reset_ev

static

Definition at line 167 of file ins_int.c.

Referenced by ins_int_init().

vel_est_ev

abi_event vel_est_ev

static

Definition at line 141 of file ins_int.c.

Referenced by ins_int_init().