ins_gps_passthrough.c File Reference

Simply converts GPS ECEF position and velocity to NED and passes it through to the state interface. More...

#include "subsystems/ins/ins_gps_passthrough.h"
#include "subsystems/ins.h"
#include <inttypes.h>
#include <math.h>
#include "state.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for ins_gps_passthrough.c:

Go to the source code of this file.

Data Structures
struct	InsGpsPassthrough

Macros
#define	USE_INS_NAV_INIT   TRUE
#define	INS_PT_IMU_ID   ABI_BROADCAST
	ABI bindings on ACCEL data. More...
#define	INS_PT_GPS_ID   GPS_MULTI_ID
	ABI binding for gps data. More...

Functions
static void	accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void	body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i_f)
static void	gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
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)
void	ins_gps_passthrough_init (void)
void	ins_reset_local_origin (void)
	INS local origin reset. More...
void	ins_reset_altitude_ref (void)
	INS altitude reference reset. More...

Variables
struct InsGpsPassthrough	ins_gp
static abi_event	accel_ev
static abi_event	body_to_imu_ev
static struct OrientationReps	body_to_imu
static abi_event	gps_ev

Detailed Description

Simply converts GPS ECEF position and velocity to NED and passes it through to the state interface.

Definition in file ins_gps_passthrough.c.

---

Data Structure Documentation

struct InsGpsPassthrough

Definition at line 49 of file ins_gps_passthrough.c.

Collaboration diagram for InsGpsPassthrough:

Data Fields
struct NedCoor_i	ltp_accel
struct LtpDef_i	ltp_def
bool	ltp_initialized
struct NedCoor_i	ltp_pos
struct NedCoor_i	ltp_speed

Macro Definition Documentation

#define INS_PT_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 78 of file ins_gps_passthrough.c.

Referenced by ins_gps_passthrough_init().

#define INS_PT_IMU_ID   ABI_BROADCAST

ABI bindings on ACCEL data.

Definition at line 64 of file ins_gps_passthrough.c.

Referenced by ins_gps_passthrough_init().

#define USE_INS_NAV_INIT   TRUE

Definition at line 40 of file ins_gps_passthrough.c.

Function Documentation

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

static

Definition at line 198 of file ins_gps_passthrough.c.

References ACCEL_BFP_OF_REAL, body_to_imu, ins_gp, int32_rmat_transp_vmult(), InsGpsPassthrough::ltp_accel, orientationGetRMat_i(), stateGetNedToBodyRMat_i(), stateSetAccelNed_i(), VECT3_COPY, and Int32Vect3::z.

Referenced by ins_gps_passthrough_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 213 of file ins_gps_passthrough.c.

References body_to_imu, and orientationSetQuat_f().

Referenced by ins_gps_passthrough_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 83 of file ins_gps_passthrough.c.

References GPS_FIX_3D, ins_gp, ins_reset_local_origin(), 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, InsGpsPassthrough::ltp_def, InsGpsPassthrough::ltp_initialized, InsGpsPassthrough::ltp_pos, InsGpsPassthrough::ltp_speed, ned_of_ecef_point_i(), ned_of_ecef_vect_i(), stateSetPositionNed_i(), and stateSetSpeedNed_i().

Referenced by ins_gps_passthrough_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_gps_passthrough_init

(

void

)

Definition at line 140 of file ins_gps_passthrough.c.

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 186 of file ins_gps_passthrough.c.

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

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 177 of file ins_gps_passthrough.c.

References LlaCoor_i::alt, GpsState::ecef_pos, gps, GpsState::hmsl, LtpDef_i::hmsl, ins_gp, LtpDef_i::lla, GpsState::lla_pos, InsGpsPassthrough::ltp_def, ltp_def_from_ecef_i(), InsGpsPassthrough::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:

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

static

Definition at line 112 of file ins_gps_passthrough.c.

References ins_gp, InsGpsPassthrough::ltp_accel, InsGpsPassthrough::ltp_pos, InsGpsPassthrough::ltp_speed, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_gps_passthrough_init().

Here is the caller graph for this function:

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

static

Definition at line 128 of file ins_gps_passthrough.c.

References LlaCoor_i::alt, LtpDef_i::ecef, LtpDef_i::hmsl, ins_gp, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsGpsPassthrough::ltp_def, InsGpsPassthrough::ltp_initialized, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_gps_passthrough_init().

Here is the caller graph for this function:

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

static

Definition at line 120 of file ins_gps_passthrough.c.

References ins_gp, InsGpsPassthrough::ltp_accel, InsGpsPassthrough::ltp_pos, InsGpsPassthrough::ltp_speed, and NedCoor_i::z.

Referenced by ins_gps_passthrough_init().

Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev

static

Definition at line 66 of file ins_gps_passthrough.c.

struct OrientationReps body_to_imu

static

Definition at line 71 of file ins_gps_passthrough.c.

Referenced by accel_cb(), and body_to_imu_cb().

abi_event body_to_imu_ev

static

Definition at line 69 of file ins_gps_passthrough.c.

abi_event gps_ev

static

Definition at line 81 of file ins_gps_passthrough.c.

Referenced by ins_gps_passthrough_init().

struct InsGpsPassthrough ins_gp

Definition at line 59 of file ins_gps_passthrough.c.

Referenced by accel_cb(), gps_cb(), ins_gps_passthrough_init(), ins_reset_altitude_ref(), ins_reset_local_origin(), send_ins(), send_ins_ref(), and send_ins_z().