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 "generated/flight_plan.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_MAX_PROPAGATION_STEPS 200
	maximum number of propagation steps without any updates in between More...

#define	USE_INS_NAV_INIT TRUE

#define	INS_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_VEL_ID ABI_BROADCAST
	ABI binding for VELOCITY_ESTIMATE. More...

Functions
static void	baro_cb (uint8_t sender_id, float pressure)

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

static void	ins_init_origin_from_flightplan (void)
	initialize the local origin (ltp_def) from flight plan position More...

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)

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)

void	ins_int_register (void)

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_BARO_ID ABI_BROADCAST

default barometer to use in INS

Definition at line 120 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 131 of file ins_int.c.

Referenced by ins_int_register().

#define INS_INT_VEL_ID ABI_BROADCAST

ABI binding for VELOCITY_ESTIMATE.

Usually this is coming from opticflow.

Definition at line 141 of file ins_int.c.

Referenced by ins_int_register().

#define INS_MAX_PROPAGATION_STEPS 200

maximum number of propagation steps without any updates in between

Definition at line 103 of file ins_int.c.

Referenced by ins_int_init(), and ins_int_propagate().

#define INS_VFF_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 107 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 497 of file ins_int.c.

References ins_int_propagate(), and PRINT_CONFIG_MSG().

Referenced by ins_int_register().

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

Definition at line 308 of file ins_int.c.

References InsInt::baro_initialized, InsInt::baro_z, FALSE, ins_int, ins_ned_to_state(), ins_update_from_vff(), pprz_isa_height_of_pressure(), InsInt::propagation_cnt, InsInt::qfe, TRUE, 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:

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

static

Definition at line 511 of file ins_int.c.

References ins_int_update_gps().

Referenced by ins_int_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static void ins_init_origin_from_flightplan ( void )

static

initialize the local origin (ltp_def) from flight plan position

Definition at line 443 of file ins_int.c.

References LlaCoor_i::alt, ecef_of_lla_i(), LtpDef_i::hmsl, ins_int, LlaCoor_i::lat, LlaCoor_i::lon, InsInt::ltp_def, ltp_def_from_ecef_i(), and stateSetLocalOrigin_i().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_int_init ( void )

Definition at line 184 of file ins_int.c.

References b2_hff_init(), baro_cb(), InsInt::baro_initialized, DefaultPeriodic, FALSE, InsInt::hf_realign, INS_BARO_ID, ins_init_origin_from_flightplan(), ins_int, INS_MAX_PROPAGATION_STEPS, INT32_VECT3_ZERO, InsInt::ltp_accel, InsInt::ltp_initialized, InsInt::ltp_pos, InsInt::ltp_speed, InsInt::propagation_cnt, register_periodic_telemetry(), sonar_cb(), TRUE, InsInt::vf_reset, and vff_init_zero().

Referenced by ins_int_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_int_propagate	(	struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 264 of file ins_int.c.

References ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, b2_hff_propagate(), Imu::body_to_imu, imu, ins_int, INS_MAX_PROPAGATION_STEPS, ins_ned_to_state(), ins_update_from_vff(), int32_rmat_transp_vmult(), InsInt::ltp_accel, orientationGetRMat_i(), InsInt::propagation_cnt, stateGetNedToBodyRMat_i(), vff_propagate(), NedCoor_i::x, Int32Vect3::x, NedCoor_i::y, Int32Vect3::y, NedCoor_i::z, and Int32Vect3::z.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_int_register ( void )

Definition at line 549 of file ins_int.c.

References ABI_BROADCAST, accel_cb(), gps_cb(), INS_INT_IMU_ID, ins_int_init(), INS_INT_VEL_ID, ins_register_impl(), and vel_est_cb().

Here is the call graph for this function:

void ins_int_update_gps ( struct GpsState * gps_s )

Todo:: maybe use gps_s->ned_vel directly??

Definition at line 338 of file ins_int.c.

References b2_hff_realign(), b2_hff_update_gps(), GpsState::ecef_pos, GpsState::ecef_vel, FALSE, GpsState::fix, GPS_FIX_3D, InsInt::hf_realign, ins_int, ins_ned_to_state(), ins_reset_local_origin(), INS_VFF_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, stateGetNedToBodyQuat_i(), VECT2_ASSIGN, VECT2_SDIV, VECT3_SUB, vff_update_z_conf(), NedCoor_i::x, Int32Vect3::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by gps_cb().

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

References ins_int, InsInt::ltp_accel, InsInt::ltp_pos, InsInt::ltp_speed, 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 249 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(), TRUE, 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 227 of file ins_int.c.

References LlaCoor_i::alt, GpsState::ecef_pos, FALSE, 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(), TRUE, 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 476 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 vel_est_cb	(	uint8_t	sender_id,
		uint32_t	stamp,
		float	x,
		float	y,
		float	z,
		float	noise
	)

static

Definition at line 518 of file ins_int.c.

References b2_hff_update_vel(), float_quat_vmult(), ins_int, ins_ned_to_state(), InsInt::ltp_speed, 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_register().

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

abi_event baro_ev

Definition at line 124 of file ins_int.c.

abi_event gps_ev

static

Definition at line 134 of file ins_int.c.

struct InsInt ins_int

global INS state

Definition at line 146 of file ins_int.c.

Referenced by baro_cb(), ins_init_origin_from_flightplan(), ins_int_init(), ins_int_propagate(), ins_int_update_gps(), ins_ned_to_state(), ins_reset_altitude_ref(), ins_reset_local_origin(), ins_update_from_vff(), and vel_est_cb().

abi_event vel_est_ev

static

Definition at line 143 of file ins_int.c.

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation