ins_vectornav.c File Reference

Vectornav VN-200 INS subsystem. More...

#include "subsystems/ins/ins_vectornav.h"
#include "math/pprz_geodetic_wgs84.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for ins_vectornav.c:

Go to the source code of this file.

Macros
#define	USE_INS_NAV_INIT   TRUE

Functions
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	send_vn_info (struct transport_tx *trans, struct link_device *dev)
static void	send_accel (struct transport_tx *trans, struct link_device *dev)
static void	send_gyro (struct transport_tx *trans, struct link_device *dev)
static void	send_accel_scaled (struct transport_tx *trans, struct link_device *dev)
static void	send_gyro_scaled (struct transport_tx *trans, struct link_device *dev)
	PRINT_CONFIG_MSG ("USE_INS_NAV_INIT defaulting to TRUE")
void	ins_vectornav_event (void)
	Event handling for Vectornav. More...
void	ins_vectornav_init (void)
	Initialize Vectornav struct. More...
void	ins_vectornav_read_message (void)
	Read received data. More...
void	ins_vectornav_check_status (void)
	Check INS status. More...
void	ins_vectornav_set_sacc (void)
	Set speed (velocity) uncertainty (NED) speed accuracy in cm/s. More...
void	ins_vectornav_set_pacc (void)
	Find maximum uncertainty (NED) position accuracy in cm. More...
void	ins_vectornav_yaw_pitch_roll_to_attitude (struct FloatEulers *vn_attitude)
	Convert yaw, pitch, and roll data from VectorNav to correct attitude yaw(0), pitch(1), roll(2) -> phi, theta, psi [deg] -> rad. More...
void	ins_vectornav_propagate ()
	Propagate the received states into the vehicle state machine. More...

Variables
struct InsVectornav	ins_vn

Detailed Description

Vectornav VN-200 INS subsystem.

Author

Michal Podhradsky micha.nosp@m.l.po.nosp@m.dhrad.nosp@m.sky@.nosp@m.aggie.nosp@m.mail.nosp@m..usu..nosp@m.edu

Definition in file ins_vectornav.c.

Macro Definition Documentation

#define USE_INS_NAV_INIT   TRUE

Definition at line 118 of file ins_vectornav.c.

Function Documentation

void ins_vectornav_check_status

(

void

)

Check INS status.

Definition at line 282 of file ins_vectornav.c.

References InsVectornav::err, InsVectornav::ins_status, ins_vn, and InsVectornav::mode.

Referenced by ins_vectornav_propagate().

Here is the caller graph for this function:

void ins_vectornav_event

(

void

)

Event handling for Vectornav.

Definition at line 125 of file ins_vectornav.c.

References ins_vectornav_read_message(), ins_vn, VNPacket::msg_available, vn200_event(), and InsVectornav::vn_packet.

Here is the call graph for this function:

void ins_vectornav_init

(

void

)

Initialize Vectornav struct.

Definition at line 141 of file ins_vectornav.c.

References InsVectornav::body_to_imu, VNPacket::chksm_error, DefaultPeriodic, FLOAT_VECT3_ZERO, VNPacket::framing_error, get_sys_time_float(), VNPacket::hdr_error, ins_init_origin_i_from_flightplan(), ins_vn, INS_VN_BODY_TO_IMU_PHI, INS_VN_BODY_TO_IMU_PSI, INS_VN_BODY_TO_IMU_THETA, INT32_VECT3_ZERO, InsVectornav::lin_accel, InsVectornav::ltp_accel_i, InsVectornav::ltp_def, InsVectornav::ltp_initialized, InsVectornav::ltp_pos_i, InsVectornav::ltp_speed_i, VNPacket::msg_available, VNPacket::msg_idx, VNPacket::noise_error, orientationSetEulers_f(), VNPacket::overrun_error, register_periodic_telemetry(), send_accel(), send_accel_scaled(), send_gyro(), send_gyro_scaled(), send_ins(), send_ins_ref(), send_ins_z(), send_vn_info(), VNPacket::status, InsVectornav::vel_body, InsVectornav::vel_ned, InsVectornav::vn_freq, InsVectornav::vn_packet, InsVectornav::vn_status, InsVectornav::vn_time, VNMsgSync, and VNNotTracking.

Here is the call graph for this function:

void ins_vectornav_propagate

(

void

)

Propagate the received states into the vehicle state machine.

Definition at line 336 of file ins_vectornav.c.

References InsVectornav::accel, InsVectornav::accel_i, ACCELS_BFP_OF_REAL, LlaCoor_f::alt, InsVectornav::attitude, InsVectornav::body_to_imu, GpsState::course, ECEF_BFP_OF_REAL, ecef_of_ned_point_f(), GpsState::ecef_pos, GpsState::ecef_vel, GpsState::fix, float_quat_of_eulers(), float_rmat_comp(), float_rmat_of_quat(), float_rmat_ratemult(), float_rmat_transp_vmult(), get_sys_time_usec(), gps, GPS_FIX_3D, GPS_UBX_ID, GPS_VALID_COURSE_BIT, GPS_VALID_HMSL_BIT, GPS_VALID_POS_ECEF_BIT, GPS_VALID_POS_LLA_BIT, GPS_VALID_VEL_ECEF_BIT, GpsState::gspeed, InsVectornav::gyro, InsVectornav::gyro_i, GpsState::hmsl, IMU_ASPIRIN_ID, ins_vectornav_check_status(), ins_vectornav_set_pacc(), ins_vectornav_set_sacc(), ins_vectornav_yaw_pitch_roll_to_attitude(), ins_vn, GpsState::last_3dfix_ticks, GpsState::last_3dfix_time, GpsState::last_msg_ticks, GpsState::last_msg_time, LlaCoor_f::lat, InsVectornav::lin_accel, LLA_BFP_OF_REAL, InsVectornav::lla_pos, GpsState::lla_pos, LlaCoor_f::lon, InsVectornav::ltp_accel_f, InsVectornav::ltp_accel_i, ltp_def_from_lla_f(), InsVectornav::ltp_pos_i, InsVectornav::ltp_speed_i, sys_time::nb_sec, sys_time::nb_sec_rem, orientationGetRMat_f(), InsVectornav::pos_lla, RATES_BFP_OF_REAL, stateGetAccelNed_i(), stateGetPositionEcef_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyRMat_f(), stateSetPositionLla_i(), stateSetSpeedNed_f(), GpsState::valid_fields, VECT3_ASSIGN, InsVectornav::vel_ned, wgs84_ellipsoid_to_geoid_f(), EcefCoor_i::x, FloatVect3::x, NedCoor_f::x, EcefCoor_i::y, FloatVect3::y, NedCoor_f::y, EcefCoor_i::z, and FloatVect3::z.

Referenced by ins_vectornav_read_message().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_vectornav_read_message

(

void

)

Read received data.

Definition at line 192 of file ins_vectornav.c.

References InsVectornav::accel, InsVectornav::attitude, GpsState::fix, get_sys_time_float(), gps, InsVectornav::gyro, idx, InsVectornav::ins_status, ins_vectornav_propagate(), ins_vn, InsVectornav::lin_accel, VNPacket::msg_buf, GpsState::num_sv, InsVectornav::pos_lla, InsVectornav::pos_u, InsVectornav::timestamp, GpsState::tow, InsVectornav::vel_body, InsVectornav::vel_ned, InsVectornav::vel_u, VN_HEADER_SIZE, InsVectornav::vn_packet, InsVectornav::vn_time, and InsVectornav::ypr_u.

Referenced by ins_vectornav_event().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_vectornav_set_pacc

(

void

)

Find maximum uncertainty (NED) position accuracy in cm.

Definition at line 301 of file ins_vectornav.c.

References gps, ins_vn, GpsState::pacc, and InsVectornav::pos_u.

Referenced by ins_vectornav_propagate().

Here is the caller graph for this function:

void ins_vectornav_set_sacc

(

void

)

Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.

Definition at line 292 of file ins_vectornav.c.

References gps, ins_vn, GpsState::sacc, and InsVectornav::vel_u.

Referenced by ins_vectornav_propagate().

Here is the caller graph for this function:

void ins_vectornav_yaw_pitch_roll_to_attitude

(

struct FloatEulers *

vn_attitude

)

Convert yaw, pitch, and roll data from VectorNav to correct attitude yaw(0), pitch(1), roll(2) -> phi, theta, psi [deg] -> rad.

Definition at line 320 of file ins_vectornav.c.

References FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ins_vectornav_propagate().

Here is the caller graph for this function:

PRINT_CONFIG_MSG

(

"USE_INS_NAV_INIT defaulting to TRUE"

)

Referenced by accel_cb(), adc_init(), adc_init_single(), ahrs_dcm_update_accel(), Drift_correction(), gyro_cb(), init_ltp(), mag_cb(), mcu_arch_init(), and nps_autopilot_run_step().

Here is the caller graph for this function:

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

static

Definition at line 92 of file ins_vectornav.c.

References InsVectornav::accel, ins_vn, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

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

static

Definition at line 104 of file ins_vectornav.c.

References InsVectornav::accel_i, ins_vn, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

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

static

Definition at line 98 of file ins_vectornav.c.

References InsVectornav::gyro, ins_vn, FloatRates::p, FloatRates::q, and FloatRates::r.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

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

static

Definition at line 110 of file ins_vectornav.c.

References InsVectornav::gyro_i, ins_vn, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

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

static

Definition at line 37 of file ins_vectornav.c.

References ins_vn, InsVectornav::ltp_accel_i, InsVectornav::ltp_pos_i, InsVectornav::ltp_speed_i, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_vectornav_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 51 of file ins_vectornav.c.

References LlaCoor_i::alt, LtpDef_i::ecef, LtpDef_i::hmsl, ins_vn, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsVectornav::ltp_def, InsVectornav::ltp_initialized, InsVectornav::qfe, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_vectornav_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 45 of file ins_vectornav.c.

References InsVectornav::baro_z, ins_vn, InsVectornav::ltp_accel_i, InsVectornav::ltp_pos_i, InsVectornav::ltp_speed_i, and NedCoor_i::z.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

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

static

Definition at line 61 of file ins_vectornav.c.

References VNPacket::chksm_error, VNPacket::counter, InsVectornav::err, GpsState::fix, gps, GPS_FIX_NONE, VNPacket::hdr_error, ins_vn, InsVectornav::mode, FloatEulers::phi, FloatEulers::psi, FloatEulers::theta, InsVectornav::timestamp, InsVectornav::vn_freq, InsVectornav::vn_packet, and InsVectornav::ypr_u.

Referenced by ins_vectornav_init().

Here is the caller graph for this function:

Variable Documentation

struct InsVectornav ins_vn

Definition at line 32 of file ins_vectornav.c.

Referenced by ins_vectornav_check_status(), ins_vectornav_event(), ins_vectornav_init(), ins_vectornav_propagate(), ins_vectornav_read_message(), ins_vectornav_set_pacc(), ins_vectornav_set_sacc(), send_accel(), send_accel_scaled(), send_gyro(), send_gyro_scaled(), send_ins(), send_ins_ref(), send_ins_z(), and send_vn_info().