Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_vectornav.c
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  */
31 
33 
34 #if PERIODIC_TELEMETRY
36 
37 static void send_ins(struct transport_tx *trans, struct link_device *dev)
38 {
39  pprz_msg_send_INS(trans, dev, AC_ID,
43 }
44 
45 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
46 {
47  pprz_msg_send_INS_Z(trans, dev, AC_ID,
49 }
50 
51 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
52 {
53  if (ins_vn.ltp_initialized) {
54  pprz_msg_send_INS_REF(trans, dev, AC_ID,
58  }
59 }
60 
61 static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
62 {
63  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
67  &ins_vn.vn_rate,
73 }
74 
75 static void send_accel(struct transport_tx *trans, struct link_device *dev)
76 {
77  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
79 }
80 
81 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
82 {
83  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
85 }
86 
87 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
88 {
89  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
91 }
92 
93 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
94 {
95  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
97 }
98 #endif
99 
100 #ifndef USE_INS_NAV_INIT
101 #define USE_INS_NAV_INIT TRUE
102 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE");
103 #endif
104 
105 
113 {
114  static uint16_t last_cnt = 0;
115  static uint16_t sec_cnt = 0;
116 
117  sec_cnt = ins_vn.vn_packet.counter - last_cnt;
118  ins_vn.vn_rate = sec_cnt; // update frequency counter
119 
120  // we want at least 75% of periodic frequency to be able to control the airfcraft
121  if (ins_vn.vn_rate < (PERIODIC_FREQUENCY*0.75)) {
122  gps.fix = GPS_FIX_NONE;
123  }
124 
125  // Make gps pacc available in GPS page on GCS
126  static uint32_t last_pacc = 0;
127  // update only if pacc changes
128  if (last_pacc != gps.pacc) {
129  last_pacc = gps.pacc;
130  // we don't know the value of CNO, hence oscillate
131  // between 0 and 1 to not confuse the user
132  gps.svinfos[0].cno = (gps.svinfos[0].cno + 1) % 2;
133  }
134 
135  // update counter
136  last_cnt = ins_vn.vn_packet.counter;
137 
138  // reset mode
139  ins_vn.vn_data.mode = 0;
140 }
141 
142 
147 {
148  // receive data
150 
151  // read message
156  }
157 }
158 
159 
164 {
165  // Initialize variables
167  ins_vn.vn_rate = 0;
168 
169  // Initialize packet
178 
183 
184  // initialize data struct
185  memset(&(ins_vn.vn_data), 0, sizeof(struct VNData));
186 
187 #if USE_INS_NAV_INIT
189  ins_vn.ltp_initialized = true;
190 #else
191  ins_vn.ltp_initialized = false;
192 #endif
193 
194  struct FloatEulers body_to_imu_eulers =
196  orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers);
197 
198 #if PERIODIC_TELEMETRY
202  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
204  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
205  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
206  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
207 #endif
208 }
209 
210 
211 
212 
218 {
219  gps.sacc = (uint32_t)(ins_vn.vn_data.vel_u * 100);
220 }
221 
227 {
228  float pacc = ins_vn.vn_data.pos_u[0]; // in meters
229  if (ins_vn.vn_data.pos_u[1] > pacc) {
230  pacc = ins_vn.vn_data.pos_u[1];
231  }
232  if (ins_vn.vn_data.pos_u[2] > pacc) {
233  pacc = ins_vn.vn_data.pos_u[2];
234  }
235  gps.pacc = (uint32_t)(pacc * 100);
236 }
237 
238 
239 
245 {
246  // Acceleration [m/s^2]
247  // in fixed point for sending as ABI and telemetry msgs
249 
250  // Rates [rad/s]
251  static struct FloatRates body_rate;
252  // in fixed point for sending as ABI and telemetry msgs
254  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.vn_data.gyro); // compute body rates
255  stateSetBodyRates_f(&body_rate); // Set state [rad/s]
256 
257  // Attitude [deg]
258  static struct FloatQuat imu_quat; // convert from euler to quat
260  static struct FloatRMat imu_rmat; // convert from quat to rmat
261  float_rmat_of_quat(&imu_rmat, &imu_quat);
262  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
263  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
264  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]
265 
266  // NED (LTP) velocity [m/s]
267  // North east down (NED), also known as local tangent plane (LTP),
268  // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
269  // It consists of three numbers: one represents the position along the northern axis,
270  // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
271  // up in order to comply with the right-hand rule.
272  // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
273  // x = North
274  // y = East
275  // z = Down
276  stateSetSpeedNed_f(&ins_vn.vn_data.vel_ned); // set state
277 
278  // NED (LTP) acceleration [m/s^2]
279  static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
281  static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
282  VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
283  stateSetAccelNed_f(&ltp_accel); // then set the states
284  ins_vn.ltp_accel_f = ltp_accel;
285 
286  // LLA position [rad, rad, m]
287  //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
288  ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.vn_data.pos_lla[0]); // ins_impl.pos_lla[0] = lat
289  ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.vn_data.pos_lla[1]); // ins_impl.pos_lla[1] = lon
290  ins_vn.lla_pos.alt = ((float)ins_vn.vn_data.pos_lla[2]); // ins_impl.pos_lla[2] = alt
294 
295  // ECEF velocity
296  // TODO: properly implement
297 
298  // ECEF position
303 
304  // GPS Ground speed
306  gps.gspeed = ((uint16_t)(speed * 100));
307 
308  // GPS course
309  gps.course = (int32_t)(1e7 * (atan2(ins_vn.vn_data.vel_ned.y, ins_vn.vn_data.vel_ned.x)));
311 
312  // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
313  // as a workaround
315  gps.hmsl = (int32_t)((ins_vn.lla_pos.alt - geoid_h)* 1000.0f);
317 
318  // Set GPS fix
320 
321  // Set GPS num_sv
323 
324  // set position uncertainty
326 
327  // set velocity uncertainty
329 
330  // check GPS status
333  if (gps.fix == GPS_FIX_3D) {
336  }
337 
338  // update internal states for telemetry purposes
339  // TODO: directly convert vectornav output instead of using state interface
340  // to support multiple INS running at the same time
344 }
unsigned short uint16_t
Definition: types.h:16
struct NedCoor_f ltp_accel_f
Definition: ins_vectornav.h:73
int32_t z
in centimeters
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
uint16_t overrun_error
Definition: vn200_serial.h:70
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:112
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
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.
float y
in meters
uint8_t msg_idx
Definition: vn200_serial.h:68
float phi
in radians
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:45
struct VNPacket vn_packet
Packet struct.
Definition: ins_vectornav.h:80
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
#define INS_VN_BODY_TO_IMU_PSI
Definition: ins_vectornav.h:59
struct NedCoor_i ltp_accel_i
Definition: ins_vectornav.h:71
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
Definition: vn200_serial.h:103
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition: state.h:656
Periodic telemetry system header (includes downlink utility and generated code).
uint32_t chksm_error
Definition: vn200_serial.h:64
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
int32_t y
in centimeters
uint16_t counter
Definition: vn200_serial.h:75
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
float r
in rad/s
#define INT32_VECT3_ZERO(_v)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
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.
Definition: ins_vectornav.h:82
uint32_t hdr_error
Definition: vn200_serial.h:65
float psi
in radians
uint16_t noise_error
Definition: vn200_serial.h:71
bool msg_available
Definition: vn200_serial.h:63
#define INS_VN_BODY_TO_IMU_THETA
Definition: ins_vectornav.h:58
uint8_t num_sv
number of visible satellites
Definition: vn200_serial.h:100
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float q
in rad/s
float p
in rad/s
Vectornav VN-200 INS subsystem.
int32_t z
Down.
struct FloatVect3 lin_accel
Linear acceleration in imu frame [m/s^2].
Definition: vn200_serial.h:93
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:110
uint8_t gps_fix
None|2D|3D.
Definition: vn200_serial.h:101
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
Definition: gps.h:114
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:88
int32_t alt
in millimeters above WGS84 reference ellipsoid
WGS-84 Geoid Heights.
euler angles
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
Roation quaternion.
int32_t y
East.
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
float theta
in radians
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.
Definition: vn200_serial.h:91
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
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
Definition: vn200_serial.h:96
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
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.
Definition: gps.h:81
vector in North East Down coordinates Units: meters
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: state.h:547
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
uint16_t vn_rate
data frequency
Definition: ins_vectornav.h:83
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
Definition: vn200_serial.c:180
struct LtpDef_i ltp_def
Definition: ins_vectornav.h:65
struct Int32Rates gyro_i
Definition: ins_vectornav.h:87
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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
Definition: ins.c:39
struct LlaCoor_f lla_pos
Definition: ins_vectornav.h:72
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_vectornav.h:76
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:93
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
bool ltp_initialized
Definition: ins_vectornav.h:66
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:51
signed long int32_t
Definition: types.h:19
struct VNData vn_data
Data struct.
Definition: ins_vectornav.h:81
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
float alt
in meters (normally above WGS84 reference ellipsoid)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:87
uint16_t framing_error
Definition: vn200_serial.h:72
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
struct InsVectornav ins_vn
Definition: ins_vectornav.c:32
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
struct Int32Vect3 accel_i
Definition: ins_vectornav.h:86
struct NedCoor_i ltp_speed_i
Definition: ins_vectornav.h:70
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_vectornav.h:90
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
float vel_u
NED velocity uncertainty [m/s].
Definition: vn200_serial.h:92
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
#define LLA_BFP_OF_REAL(_o, _i)
int32_t p
in rad/s with INT32_RATE_FRAC
float lon
in radians
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:75
struct NedCoor_i ltp_pos_i
Definition: ins_vectornav.h:69
rotation matrix
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
#define INS_VN_BODY_TO_IMU_PHI
Definition: ins_vectornav.h:57
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
int32_t x
in centimeters
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
double pos_lla[3]
Definition: vn200_serial.h:102
float lat
in radians
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
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).
Definition: state.h:665
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:61
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
int32_t lat
in degrees*1e7
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
uint8_t fix
status of fix
Definition: gps.h:107
enum VNMsgStatus status
Definition: vn200_serial.h:67
void ins_vectornav_event(void)
Event handling for Vectornav.
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
int32_t q
in rad/s with INT32_RATE_FRAC
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:81
struct GpsState gps
global GPS state
Definition: gps.c:69
angular rates
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:37
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
float x
in meters
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).