Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_vectornav.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
3  * Utah State University, http://aggieair.usu.edu/
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
29 #ifndef INS_VECTORNAV_H
30 #define INS_VECTORNAV_H
31 
32 // Subsystems
33 #include "subsystems/gps.h"
34 #include "subsystems/ins.h"
35 
36 // Peripheral
38 
39 // Geodetic / Math
40 #include "math/pprz_algebra.h"
41 #include "math/pprz_geodetic_int.h"
43 #include "math/pprz_geodetic_int.h"
44 #include "math/pprz_isa.h"
45 
46 // Generated
47 #include "generated/airframe.h"
48 
49 // Systime
50 #include "mcu_periph/sys_time.h"
51 
52 // Abi
53 #include "subsystems/abi.h"
54 
55 
56 #if !defined INS_VN_BODY_TO_IMU_PHI && !defined INS_VN_BODY_TO_IMU_THETA && !defined INS_VN_BODY_TO_IMU_PSI
57 #define INS_VN_BODY_TO_IMU_PHI 0
58 #define INS_VN_BODY_TO_IMU_THETA 0
59 #define INS_VN_BODY_TO_IMU_PSI 0
60 #endif
61 
62 
63 // Ins implementation state (fixed point)
64 struct InsVectornav {
65  struct LtpDef_i ltp_def; // initial position
66  bool ltp_initialized; // status indicator
67 
68  // output LTP NED for telemetry messages
72 
74 
76 
77  // baro [height above ground]
78  float baro_z;
79  float qfe;
80 
81  // Packet data
84  float vn_freq;
88 
89  // Auxilliary data fields
90  float timestamp;
92  // rates -> imu
93  double pos_lla[3]; // Lla [deg, deg, m above elipsoid]
94  struct NedCoor_f vel_ned;
95  // accel -> imu
96  // num sats -> GPS
97  // GPS fix -> GPS
98  float pos_u[3];
99  float vel_u;
106  struct FloatVect3 accel;
107  struct FloatRates gyro;
108 
109  // in fixed point for sending as ABI and telemetry msgs
112 
115 };
116 
117 
118 // global INS state
119 extern struct InsVectornav ins_vn;
120 
121 extern void ins_vectornav_init(void);
122 extern void ins_vectornav_event(void);
123 
124 extern void ins_vectornav_read_message(void);
125 extern void ins_vectornav_check_status(void);
126 extern void ins_vectornav_set_sacc(void);
127 extern void ins_vectornav_set_pacc(void);
128 extern void ins_vectornav_propagate(void);
129 extern void ins_vectornav_yaw_pitch_roll_to_attitude(struct FloatEulers *vn_attitude);
130 
131 #endif /* INS_VECTORNAV_H */
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.
double pos_lla[3]
Definition: ins_vectornav.h:93
unsigned short uint16_t
Definition: types.h:16
struct NedCoor_f ltp_accel_f
Definition: ins_vectornav.h:75
angular rates
definition of the local (flat earth) coordinate system
void ins_vectornav_read_message(void)
Read received data.
struct VNPacket vn_packet
Packet struct.
Definition: ins_vectornav.h:82
uint8_t err
see page 122 of VN-200 datasheet
struct NedCoor_i ltp_accel_i
Definition: ins_vectornav.h:71
void ins_vectornav_check_status(void)
Check INS status.
uint32_t vn_ltime
aux time stamp
Definition: ins_vectornav.h:87
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Main include for ABI (AirBorneInterface).
enum VNStatus vn_status
VN status.
Definition: ins_vectornav.h:83
struct FloatVect3 lin_accel
Linear acceleration in imu frame [m/s^2].
Integrated Navigation System interface.
uint16_t vn_chksm
aux variable for checksum
Definition: ins_vectornav.h:85
struct FloatEulers attitude
Attitude, float, [degrees], yaw, pitch, roll.
Definition: ins_vectornav.h:91
struct FloatRates gyro
Rates in the imu frame m/s.
euler angles
vector in Latitude, Longitude and Altitude
void ins_vectornav_event(void)
Event handling for Vectornav.
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
Architecture independent timing functions.
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
void ins_vectornav_init(void)
Initialize Vectornav struct.
Vectornav VN-200 INS subsystem.
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Paparazzi generic algebra macros.
float pos_u[3]
The current GPS position uncertainty in the North East Down (NED) coordinate frame, given in meters.
Definition: ins_vectornav.h:98
Device independent GPS code (interface)
Paparazzi atmospheric pressure conversion utilities.
unsigned long uint32_t
Definition: types.h:18
struct LtpDef_i ltp_def
Definition: ins_vectornav.h:65
struct Int32Rates gyro_i
struct LlaCoor_f lla_pos
Definition: ins_vectornav.h:73
VNStatus
Definition: vn200_serial.h:71
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_vectornav.h:78
float vel_u
NED velocity uncertainty [m/s].
Definition: ins_vectornav.h:99
Paparazzi fixed point math for geodetic calculations.
bool ltp_initialized
Definition: ins_vectornav.h:66
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
void ins_vectornav_propagate(void)
Propagate the received states into the vehicle state machine.
unsigned char uint8_t
Definition: types.h:14
struct Int32Vect3 accel_i
vector in North East Down coordinates
struct NedCoor_i ltp_speed_i
Definition: ins_vectornav.h:70
struct OrientationReps body_to_imu
body_to_imu rotation
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
struct InsVectornav ins_vn
Definition: ins_vectornav.c:32
uint16_t ins_status
see page 122 of VN-200 datasheet
struct NedCoor_i ltp_pos_i
Definition: ins_vectornav.h:69
float vn_freq
data frequency
Definition: ins_vectornav.h:84
struct FloatVect3 vel_body
The estimated velocity in the imu frame, given in m/s.
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
Definition: ins_vectornav.h:94
uint32_t vn_time
VN time stamp.
Definition: ins_vectornav.h:86
angular rates
float timestamp
System time [s].
Definition: ins_vectornav.h:90