34 #if PERIODIC_TELEMETRY
37 static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
39 pprz_msg_send_INS(trans, dev, AC_ID,
45 static void send_ins_z(
struct transport_tx *trans,
struct link_device *
dev)
47 pprz_msg_send_INS_Z(trans, dev, AC_ID,
54 pprz_msg_send_INS_REF(trans, dev, AC_ID,
63 pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
75 static void send_accel(
struct transport_tx *trans,
struct link_device *
dev)
77 pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
81 static void send_gyro(
struct transport_tx *trans,
struct link_device *
dev)
83 pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
89 pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
95 pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
100 #ifndef USE_INS_NAV_INIT
101 #define USE_INS_NAV_INIT TRUE
198 #if PERIODIC_TELEMETRY
262 static struct FloatRMat ltp_to_body_rmat;
282 VECT3_ASSIGN(ltp_accel, accel_meas_ltp.
x, accel_meas_ltp.
y, accel_meas_ltp.
z);
struct NedCoor_f ltp_accel_f
#define RATES_BFP_OF_REAL(_ri, _rf)
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
Quaternion from Euler angles.
void ins_vectornav_propagate()
Propagate the received states into the vehicle state machine.
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
uint32_t pacc
position accuracy in cm
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
struct VNPacket vn_packet
Packet struct.
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
#define INS_VN_BODY_TO_IMU_PSI
struct NedCoor_i ltp_accel_i
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define INT32_VECT3_ZERO(_v)
#define VECT3_ASSIGN(_a, _x, _y, _z)
void ins_vectornav_monitor(void)
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
enum VNStatus vn_status
VN status.
#define INS_VN_BODY_TO_IMU_THETA
uint8_t num_sv
number of visible satellites
#define GPS_FIX_3D
3D GPS fix
Vectornav VN-200 INS subsystem.
struct FloatVect3 lin_accel
Linear acceleration in imu frame [m/s^2].
void vn200_event(struct VNPacket *vnp)
uint8_t gps_fix
None|2D|3D.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define ACCELS_BFP_OF_REAL(_ef, _ei)
uint32_t sacc
speed accuracy in cm/s
uint32_t last_msg_time
cpu time in sec at last received GPS message
#define GPS_VALID_COURSE_BIT
int32_t r
in rad/s with INT32_RATE_FRAC
float pos_u[3]
The current GPS position uncertainty in the North East Down (NED) coordinate frame, given in meters.
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
int32_t hmsl
height above mean sea level (MSL) in mm
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
vector in North East Down coordinates Units: meters
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
#define GPS_FIX_NONE
No GPS fix.
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
struct EcefCoor_i ecef_pos
position in ECEF in cm
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
uint16_t vn_rate
data frequency
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
#define GPS_VALID_HMSL_BIT
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
int32_t lon
in degrees*1e7
void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
float baro_z
z-position calculated from baro in meters (z-down)
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
struct FloatRates gyro
Rates in the imu frame m/s.
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
struct VNData vn_data
Data struct.
static const struct usb_device_descriptor dev
float alt
in meters (normally above WGS84 reference ellipsoid)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
struct InsVectornav ins_vn
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
struct Int32Vect3 accel_i
struct NedCoor_i ltp_speed_i
struct OrientationReps body_to_imu
body_to_imu rotation
volatile uint32_t nb_sec
full seconds since startup
float vel_u
NED velocity uncertainty [m/s].
#define GPS_VALID_POS_ECEF_BIT
#define LLA_BFP_OF_REAL(_o, _i)
int32_t p
in rad/s with INT32_RATE_FRAC
static void send_accel(struct transport_tx *trans, struct link_device *dev)
struct NedCoor_i ltp_pos_i
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
#define INS_VN_BODY_TO_IMU_PHI
#define GPS_VALID_POS_LLA_BIT
uint8_t num_sv
number of sat in fix
uint16_t gspeed
norm of 2d ground speed in cm/s
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
float timestamp
Time since VN startup [s].
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
uint8_t err
see page 122 of VN-200 datasheet
int32_t lat
in degrees*1e7
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
void ins_vectornav_event(void)
Event handling for Vectornav.
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
int32_t q
in rad/s with INT32_RATE_FRAC
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
struct GpsState gps
global GPS state
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
void ins_vectornav_init(void)
Initialize Vectornav struct.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).