Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_vectornav.c File Reference

Vectornav VN-200 INS subsystem. More...

+ 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_monitor (void)
 Monitors vectornav data rate and changes GPS lock if the data rate is too low. More...
 
void ins_vectornav_event (void)
 Event handling for Vectornav. More...
 
void ins_vectornav_init (void)
 Initialize Vectornav struct. 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_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

◆ USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

Definition at line 105 of file ins_vectornav.c.

Function Documentation

◆ ins_vectornav_event()

void ins_vectornav_event ( void  )

Event handling for Vectornav.

Definition at line 150 of file ins_vectornav.c.

References ins_vectornav_propagate(), ins_vn, VNPacket::msg_available, vn200_event(), vn200_read_message(), InsVectornav::vn_data, and InsVectornav::vn_packet.

+ Here is the call graph for this function:

◆ ins_vectornav_init()

◆ ins_vectornav_monitor()

void ins_vectornav_monitor ( void  )

Monitors vectornav data rate and changes GPS lock if the data rate is too low.

Definition at line 116 of file ins_vectornav.c.

References SVinfo::cno, VNPacket::counter, GpsState::fix, gps, GPS_FIX_NONE, ins_vn, VNData::mode, GpsState::pacc, GpsState::svinfos, InsVectornav::vn_data, InsVectornav::vn_packet, and InsVectornav::vn_rate.

◆ ins_vectornav_propagate()

void ins_vectornav_propagate ( void  )

Propagate the received states into the vehicle state machine.

Definition at line 248 of file ins_vectornav.c.

References VNData::accel, InsVectornav::accel_i, ACCELS_BFP_OF_REAL, LlaCoor_f::alt, VNData::attitude, InsVectornav::body_to_imu, GpsState::course, GpsState::ecef_pos, GpsState::fix, float_quat_of_eulers(), float_rmat_comp(), float_rmat_of_quat(), float_rmat_ratemult(), float_rmat_transp_vmult(), gps, VNData::gps_fix, GPS_FIX_3D, GPS_VALID_COURSE_BIT, GPS_VALID_HMSL_BIT, GPS_VALID_POS_ECEF_BIT, GPS_VALID_POS_LLA_BIT, GpsState::gspeed, VNData::gyro, InsVectornav::gyro_i, GpsState::hmsl, ins_vectornav_set_pacc(), ins_vectornav_set_sacc(), ins_vn, GpsState::last_3dfix_ticks, GpsState::last_3dfix_time, GpsState::last_msg_ticks, GpsState::last_msg_time, LlaCoor_f::lat, VNData::lin_accel, LLA_BFP_OF_REAL, GpsState::lla_pos, InsVectornav::lla_pos, LlaCoor_f::lon, InsVectornav::ltp_accel_f, InsVectornav::ltp_accel_i, InsVectornav::ltp_pos_i, InsVectornav::ltp_speed_i, sys_time::nb_sec, sys_time::nb_sec_rem, GpsState::num_sv, VNData::num_sv, orientationGetRMat_f(), VNData::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, VNData::vel_ned, InsVectornav::vn_data, wgs84_ellipsoid_to_geoid_f(), FloatVect3::x, NedCoor_f::x, EcefCoor_i::x, FloatVect3::y, NedCoor_f::y, EcefCoor_i::y, FloatVect3::z, and EcefCoor_i::z.

Referenced by ins_vectornav_event().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ ins_vectornav_set_pacc()

void ins_vectornav_set_pacc ( void  )

Find maximum uncertainty (NED) position accuracy in cm.

Definition at line 230 of file ins_vectornav.c.

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

Referenced by ins_vectornav_propagate().

+ Here is the caller graph for this function:

◆ ins_vectornav_set_sacc()

void ins_vectornav_set_sacc ( void  )

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

Definition at line 221 of file ins_vectornav.c.

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

Referenced by ins_vectornav_propagate().

+ Here is the caller graph for this function:

◆ PRINT_CONFIG_MSG()

PRINT_CONFIG_MSG ( "USE_INS_NAV_INIT defaulting to TRUE )

◆ send_accel()

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

Definition at line 75 of file ins_vectornav.c.

References VNData::accel, dev, IMU_VECTORNAV_ID, ins_vn, InsVectornav::vn_data, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_vectornav_init().

+ Here is the caller graph for this function:

◆ send_accel_scaled()

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

Definition at line 89 of file ins_vectornav.c.

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

Referenced by ins_vectornav_init().

+ Here is the caller graph for this function:

◆ send_gyro()

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

Definition at line 82 of file ins_vectornav.c.

References dev, VNData::gyro, IMU_VECTORNAV_ID, ins_vn, FloatRates::p, FloatRates::q, FloatRates::r, and InsVectornav::vn_data.

Referenced by ins_vectornav_init().

+ Here is the caller graph for this function:

◆ send_gyro_scaled()

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

Definition at line 96 of file ins_vectornav.c.

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

Referenced by ins_vectornav_init().

+ 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 37 of file ins_vectornav.c.

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

◆ send_ins_ref()

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

◆ send_ins_z()

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

◆ send_vn_info()

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

Variable Documentation

◆ ins_vn