ins_skeleton.c File Reference

#include "modules/ins/ins_skeleton.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "state.h"

Include dependency graph for ins_skeleton.c:

Go to the source code of this file.

Macros
#define	USE_INS_NAV_INIT   TRUE
#define	INS_MODULE_BARO_ID   ABI_BROADCAST
	baro More...
#define	INS_MODULE_IMU_ID   ABI_BROADCAST
	IMU (accel, body_to_imu) More...
#define	INS_MODULE_GPS_ID   GPS_MULTI_ID
	ABI binding for gps data. More...

Functions
void	ins_module_wrapper_init (void)
static void	ins_ned_to_state (void)
	copy position and speed to state interface More...
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	body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i_f)
void WEAK	ins_module_init (void)
void WEAK	ins_module_update_baro (float pressure)
void WEAK	ins_module_update_gps (struct GpsState *gps_s, float dt)
void WEAK	ins_module_propagate (struct Int32Vect3 *accel, float dt)
void WEAK	ins_module_reset_local_origin (void)
void	ins_reset_local_origin (void)
	INS local origin reset. More...

Variables
static abi_event	baro_ev
static abi_event	accel_ev
static abi_event	gps_ev
static abi_event	body_to_imu_ev
struct InsModuleInt	ins_module
	global INS state More...

Detailed Description

Paparazzi specific wrapper to run simple module based INS.

Definition in file ins_skeleton.c.

Macro Definition Documentation

INS_MODULE_BARO_ID

#define INS_MODULE_BARO_ID   ABI_BROADCAST

baro

Definition at line 47 of file ins_skeleton.c.

INS_MODULE_GPS_ID

#define INS_MODULE_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 62 of file ins_skeleton.c.

INS_MODULE_IMU_ID

#define INS_MODULE_IMU_ID   ABI_BROADCAST

IMU (accel, body_to_imu)

Definition at line 54 of file ins_skeleton.c.

USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

Definition at line 35 of file ins_skeleton.c.

Function Documentation

accel_cb()

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

static

Definition at line 100 of file ins_skeleton.c.

References ins_module_propagate(), and ins_ned_to_state().

Referenced by ins_module_wrapper_init().

Here is the call graph for this function:

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 93 of file ins_skeleton.c.

References ins_module_update_baro(), and ins_ned_to_state().

Referenced by ins_module_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

body_to_imu_cb()

static void body_to_imu_cb ( uint8_t  sender_id, struct FloatQuat *  q_b2i_f  )

static

Definition at line 140 of file ins_skeleton.c.

References InsModuleInt::body_to_imu, ins_module, and orientationSetQuat_f().

Referenced by ins_module_wrapper_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 115 of file ins_skeleton.c.

References GpsState::fix, InsModuleInt::gps, GPS_FIX_3D, ins_module, ins_module_update_gps(), ins_ned_to_state(), ins_reset_local_origin(), and InsModuleInt::ltp_initialized.

Referenced by ins_module_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_module_init()

void WEAK ins_module_init

(

void

)

Definition at line 149 of file ins_skeleton.c.

Referenced by ins_module_wrapper_init().

Here is the caller graph for this function:

ins_module_propagate()

void WEAK ins_module_propagate	(	struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 185 of file ins_skeleton.c.

References InsModuleInt::body_to_imu, ins_module, int32_rmat_transp_vmult(), InsModuleInt::ltp_accel, orientationGetRMat_i(), stateGetNedToBodyRMat_i(), stateSetAccelBody_i(), and VECT3_COPY.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_module_reset_local_origin()

void WEAK ins_module_reset_local_origin

(

void

)

Definition at line 198 of file ins_skeleton.c.

Referenced by ins_reset_local_origin().

Here is the caller graph for this function:

ins_module_update_baro()

void WEAK ins_module_update_baro

(

float

pressure

)

Definition at line 153 of file ins_skeleton.c.

Referenced by baro_cb().

Here is the caller graph for this function:

ins_module_update_gps()

void WEAK ins_module_update_gps	(	struct GpsState *	gps_s,
		float	dt
	)

Definition at line 157 of file ins_skeleton.c.

References GpsState::ecef_pos, GpsState::ecef_vel, GPS_VALID_POS_ECEF_BIT, GPS_VALID_VEL_ECEF_BIT, GPS_VALID_VEL_NED_BIT, ins_module, INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, INT32_SPEED_OF_CM_S_DEN, INT32_SPEED_OF_CM_S_NUM, INT32_VECT3_SCALE_2, InsModuleInt::ltp_def, InsModuleInt::ltp_pos, InsModuleInt::ltp_speed, ned_of_ecef_point_i(), ned_of_ecef_vect_i(), GpsState::ned_vel, and GpsState::valid_fields.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_module_wrapper_init()

void ins_module_wrapper_init

(

void

)

Definition at line 223 of file ins_skeleton.c.

References accel_cb(), accel_ev, baro_cb(), baro_ev, body_to_imu_cb(), body_to_imu_ev, gps_cb(), gps_ev, ins_init_origin_i_from_flightplan(), ins_module, INS_MODULE_BARO_ID, INS_MODULE_GPS_ID, INS_MODULE_IMU_ID, ins_module_init(), INT32_VECT3_ZERO, InsModuleInt::ltp_accel, InsModuleInt::ltp_def, InsModuleInt::ltp_initialized, InsModuleInt::ltp_pos, and InsModuleInt::ltp_speed.

Here is the call 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 76 of file ins_skeleton.c.

References ins_module, InsModuleInt::ltp_accel, InsModuleInt::ltp_pos, InsModuleInt::ltp_speed, nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_i(), stateSetPositionNed_i(), and stateSetSpeedNed_i().

Referenced by accel_cb(), baro_cb(), and gps_cb().

Here is the call graph for this function:

Here is the caller 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 207 of file ins_skeleton.c.

References LlaCoor_i::alt, GpsState::ecef_pos, GpsState::fix, InsModuleInt::gps, GPS_FIX_3D, GpsState::hmsl, LtpDef_i::hmsl, ins_module, ins_module_reset_local_origin(), LtpDef_i::lla, GpsState::lla_pos, InsModuleInt::ltp_def, ltp_def_from_ecef_i(), InsModuleInt::ltp_initialized, and stateSetLocalOrigin_i().

Referenced by gps_cb().

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 67 of file ins_skeleton.c.

Referenced by ins_module_wrapper_init().

baro_ev

abi_event baro_ev

static

Definition at line 66 of file ins_skeleton.c.

Referenced by ins_module_wrapper_init().

body_to_imu_ev

abi_event body_to_imu_ev

static

Definition at line 69 of file ins_skeleton.c.

Referenced by ins_module_wrapper_init().

gps_ev

abi_event gps_ev

static

Definition at line 68 of file ins_skeleton.c.

Referenced by ins_module_wrapper_init().

ins_module

struct InsModuleInt ins_module

global INS state

Definition at line 71 of file ins_skeleton.c.

Referenced by body_to_imu_cb(), gps_cb(), ins_module_propagate(), ins_module_update_gps(), ins_module_wrapper_init(), ins_ned_to_state(), and ins_reset_local_origin().