Paparazzi UAS
v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
|
Vectornav VN-200 INS subsystem. More...
#include "subsystems/gps.h"
#include "subsystems/ins.h"
#include "peripherals/vn200_serial.h"
#include "math/pprz_algebra.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_isa.h"
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/abi.h"
Go to the source code of this file.
Data Structures | |
struct | InsVectornav |
Macros | |
#define | INS_VN_BODY_TO_IMU_PHI 0 |
#define | INS_VN_BODY_TO_IMU_THETA 0 |
#define | INS_VN_BODY_TO_IMU_PSI 0 |
Functions | |
void | ins_vectornav_init (void) |
Initialize Vectornav struct. More... | |
void | ins_vectornav_event (void) |
Event handling for Vectornav. 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 (void) |
Propagate the received states into the vehicle state machine. More... | |
void | ins_vectornav_monitor (void) |
Monitors vectornav data rate and changes GPS lock if the data rate is too low. More... | |
Variables | |
struct InsVectornav | ins_vn |
Vectornav VN-200 INS subsystem.
Definition in file ins_vectornav.h.
struct InsVectornav |
Definition at line 64 of file ins_vectornav.h.
Data Fields | ||
---|---|---|
struct Int32Vect3 | accel_i | |
float | baro_z | z-position calculated from baro in meters (z-down) |
struct OrientationReps | body_to_imu | body_to_imu rotation |
struct Int32Rates | gyro_i | |
struct LlaCoor_f | lla_pos | |
struct NedCoor_f | ltp_accel_f | |
struct NedCoor_i | ltp_accel_i | |
struct LtpDef_i | ltp_def | |
bool | ltp_initialized | |
struct NedCoor_i | ltp_pos_i | |
struct NedCoor_i | ltp_speed_i | |
float | qfe | |
struct VNData | vn_data | Data struct. |
struct VNPacket | vn_packet | Packet struct. |
uint16_t | vn_rate | data frequency |
enum VNStatus | vn_status | VN status. |
#define INS_VN_BODY_TO_IMU_PHI 0 |
Definition at line 57 of file ins_vectornav.h.
Referenced by ins_vectornav_init().
#define INS_VN_BODY_TO_IMU_PSI 0 |
Definition at line 59 of file ins_vectornav.h.
Referenced by ins_vectornav_init().
#define INS_VN_BODY_TO_IMU_THETA 0 |
Definition at line 58 of file ins_vectornav.h.
Referenced by ins_vectornav_init().
void ins_vectornav_event | ( | void | ) |
Event handling for Vectornav.
Definition at line 146 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.
void ins_vectornav_init | ( | void | ) |
Initialize Vectornav struct.
Definition at line 163 of file ins_vectornav.c.
References InsVectornav::accel_i, InsVectornav::body_to_imu, VNPacket::chksm_error, DefaultPeriodic, VNPacket::framing_error, 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::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::vn_data, InsVectornav::vn_packet, InsVectornav::vn_rate, InsVectornav::vn_status, VNMsgSync, and VNNotTracking.
void ins_vectornav_monitor | ( | void | ) |
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
Definition at line 112 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.
void ins_vectornav_propagate | ( | void | ) |
Propagate the received states into the vehicle state machine.
Definition at line 244 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, InsVectornav::lla_pos, GpsState::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(), 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_event().
void ins_vectornav_set_pacc | ( | void | ) |
Find maximum uncertainty (NED) position accuracy in cm.
Definition at line 226 of file ins_vectornav.c.
References gps, ins_vn, GpsState::pacc, VNData::pos_u, and InsVectornav::vn_data.
Referenced by ins_vectornav_propagate().
void ins_vectornav_set_sacc | ( | void | ) |
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
Definition at line 217 of file ins_vectornav.c.
References gps, ins_vn, GpsState::sacc, VNData::vel_u, and InsVectornav::vn_data.
Referenced by ins_vectornav_propagate().
struct InsVectornav ins_vn |
Definition at line 32 of file ins_vectornav.c.
Referenced by ins_vectornav_event(), ins_vectornav_init(), ins_vectornav_monitor(), ins_vectornav_propagate(), 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().