Paparazzi UAS  v5.15_devel-46-gd23ed71
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
vn200_serial.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  */
30 
31 
32 void vn200_check_status(struct VNData *vn_data);
33 void vn200_yaw_pitch_roll_to_attitude(struct FloatEulers *vn_attitude);
34 
35 
40 {
41  vn_data->mode = (uint8_t)(vn_data->ins_status & 0x03);
42  vn_data->err = (uint8_t)((vn_data->ins_status >> 3) & 0x0F);
43 }
44 
45 
53 {
54  static struct FloatEulers att_rad;
55  att_rad.phi = RadOfDeg(vn_attitude->psi);
56  att_rad.theta = RadOfDeg(vn_attitude->theta);
57  att_rad.psi = RadOfDeg(vn_attitude->phi);
58 
59  vn_attitude->phi = att_rad.phi;
60  vn_attitude->theta = att_rad.theta;
61  vn_attitude->psi = att_rad.psi;
62 }
63 
64 
70 static inline unsigned short calculateCRC(unsigned char data[], unsigned int length)
71 {
72  unsigned int i;
73  unsigned short crc = 0;
74  for (i = 0; i < length; i++) {
75  crc = (unsigned char)(crc >> 8) | (crc << 8);
76  crc ^= data[i];
77  crc ^= (unsigned char)(crc & 0xff) >> 4;
78  crc ^= crc << 12;
79  crc ^= (crc & 0x00ff) << 5;
80  }
81  return crc;
82 }
83 
84 
88 static inline bool verify_chk(unsigned char data[], unsigned int length, uint16_t *calc_chk, uint16_t *rec_chk)
89 {
90  unsigned short calc_crc = calculateCRC(data, length);
91  unsigned short rec_crc = (unsigned short)(data[length] << 8 | data[length + 1]);
92  *calc_chk = (uint16_t) calc_crc;
93  *rec_chk = (uint16_t) rec_crc;
94  if (calc_crc == rec_crc) {
95  return 1;
96  } else {
97  return 0;
98  }
99 }
100 
101 
102 static inline void vn200_read_buffer(struct VNPacket *vnp)
103 {
104  while (uart_char_available(&VN_PORT) && !(vnp->msg_available)) {
105  vn200_parse(vnp, uart_getch(&VN_PORT));
106  }
107 }
108 
109 
110 void vn200_event(struct VNPacket *vnp)
111 {
112  if (uart_char_available(&VN_PORT)) {
113  vn200_read_buffer(vnp);
114  }
115 }
116 
117 
121 void vn200_parse(struct VNPacket *vnp, uint8_t c)
122 {
123  switch (vnp->status) {
124  case VNMsgSync:
125  // sync the header
126  vnp->msg_idx = 0;
127  if (c == VN_SYNC) {
128  vnp->status = VNMsgHeader;
129  } else {
130  vnp->hdr_error++;
131  }
132  break;
133  case VNMsgHeader:
134  // read header data (we expect 0x39)
135  if (c == VN_OUTPUT_GROUP) {
136  // increment idx and save current byte for checksum
137  vnp->status = VNMsgGroup;
138  vnp->msg_buf[vnp->msg_idx] = c;
139  vnp->msg_idx++;
140  } else {
141  vnp->hdr_error++;
142  vnp->status = VNMsgSync;
143  }
144  break;
145  break;
146  case VNMsgGroup:
147  // read header data
148  vnp->msg_buf[vnp->msg_idx] = c;
149  vnp->msg_idx++;
150  if (vnp->msg_idx == VN_GROUP_BYTES) {
152  vnp->status = VNMsgData;
153  }
154  break;
155  case VNMsgData:
156  vnp->msg_buf[vnp->msg_idx] = c;
157  vnp->msg_idx++;
158  if (vnp->msg_idx == (vnp->datalength + 2)) {
159  if (verify_chk(vnp->msg_buf, vnp->datalength, &(vnp->calc_chk), &(vnp->rec_chk))) {
160  vnp->msg_available = true;
161  vnp->counter++;
162  } else {
163  vnp->msg_available = false;
164  vnp->chksm_error++;
165  }
166  vnp->status = VNMsgSync;
167  }
168  break;
169  default:
170  vnp->status = VNMsgSync;
171  vnp->msg_idx = 0;
172  break;
173  }
174 }
175 
176 
180 void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
181 {
183 
184  // Timestamp [nanoseconds] since startup
185  memcpy(&vn_data->nanostamp, &vn_packet->msg_buf[idx], sizeof(uint64_t));
186  idx += sizeof(uint64_t);
187 
188  // Timestamp [s]
189  vn_data->timestamp = ((float)vn_data->nanostamp / 1000000000); // [nanoseconds to seconds]
190 
191  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
192  memcpy(&vn_data->attitude, &vn_packet->msg_buf[idx], 3 * sizeof(float));
193  idx += 3 * sizeof(float);
194 
195  // correctly rearrange YawPitchRoll to attitude (plus scale to radians)
196  vn200_yaw_pitch_roll_to_attitude(&vn_data->attitude); // convert to correct units and axis [rad]
197 
198  // Rates (imu frame), float, [rad/s]
199  memcpy(&vn_data->gyro, &vn_packet->msg_buf[idx], 3 * sizeof(float));
200  idx += 3 * sizeof(float);
201 
202  //Pos LLA, double,[deg, deg, m]
203  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
204  memcpy(&vn_data->pos_lla, &vn_packet->msg_buf[idx], 3 * sizeof(double));
205  idx += 3 * sizeof(double);
206 
207  //VelNed, float [m/s]
208  //The estimated velocity in the North East Down (NED) frame, given in m/s.
209  memcpy(&vn_data->vel_ned, &vn_packet->msg_buf[idx], 3 * sizeof(float));
210  idx += 3 * sizeof(float);
211 
212  // Accel (imu-frame), float, [m/s^-2]
213  memcpy(&vn_data->accel, &vn_packet->msg_buf[idx], 3 * sizeof(float));
214  idx += 3 * sizeof(float);
215 
216  // tow (in nanoseconds), uint64
217  memcpy(&vn_data->tow, &vn_packet->msg_buf[idx], sizeof(uint64_t));
218  idx += sizeof(uint64_t);
219  vn_data->tow = vn_data->tow / 1000000; // nanoseconds to miliseconds
220 
221  //num sats, uint8
222  vn_data->num_sv = vn_packet->msg_buf[idx];
223  idx++;
224 
225  //gps fix, uint8
226  vn_data->gps_fix = vn_packet->msg_buf[idx];
227  idx++;
228 
229  //posU, float[3]
230  memcpy(&vn_data->pos_u, &vn_packet->msg_buf[idx], 3 * sizeof(float));
231  idx += 3 * sizeof(float);
232 
233  //velU, float
234  memcpy(&vn_data->vel_u, &vn_packet->msg_buf[idx], sizeof(float));
235  idx += sizeof(float);
236 
237  //linear acceleration imu-body frame, float [m/s^2]
238  //The estimated linear acceleration (without gravity) reported in m/s^2, and given in the body frame. The
239  //acceleration measurement has been bias compensated by the onboard INS filter, and the gravity
240  //component has been removed using the current gravity reference vector model. This measurement is
241  //attitude dependent, since the attitude solution is required to map the gravity reference vector (known
242  //in the inertial NED frame), into the body frame so that it can be removed from the measurement. If the
243  //device is stationary and the onboard INS filter is tracking, the measurement nominally will read 0 in all
244  //three axes.
245  memcpy(&vn_data->lin_accel, &vn_packet->msg_buf[idx], 3 * sizeof(float));
246  idx += 3 * sizeof(float);
247 
248  //YprU, float[3]
249  memcpy(&vn_data->ypr_u, &vn_packet->msg_buf[idx], 3 * sizeof(float));
250  idx += 3 * sizeof(float);
251 
252  //instatus, uint16
253  memcpy(&vn_data->ins_status, &vn_packet->msg_buf[idx], sizeof(uint16_t));
254  idx += sizeof(uint16_t);
255 
256  vn200_check_status(vn_data); // Check INS status
257 
258  //Vel body, float [m/s]
259  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
260  memcpy(&vn_data->vel_body, &vn_packet->msg_buf[idx], sizeof(float));
261  idx += sizeof(float);
262 }
unsigned short uint16_t
Definition: types.h:16
#define VN_HEADER_SIZE
Definition: vn200_serial.h:50
static uint32_t idx
#define VN_SYNC
Definition: vn200_serial.h:45
uint8_t msg_idx
Definition: vn200_serial.h:68
float phi
in radians
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
Definition: vn200_serial.h:103
uint16_t rec_chk
Definition: vn200_serial.h:74
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:845
uint32_t chksm_error
Definition: vn200_serial.h:64
uint16_t counter
Definition: vn200_serial.h:75
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
void vn200_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.
Definition: vn200_serial.c:52
uint32_t hdr_error
Definition: vn200_serial.h:65
float psi
in radians
bool msg_available
Definition: vn200_serial.h:63
void vn200_parse(struct VNPacket *vnp, uint8_t c)
Packet Collection & state machine.
Definition: vn200_serial.c:121
uint8_t num_sv
number of visible satellites
Definition: vn200_serial.h:100
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 FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:88
static void vn200_read_buffer(struct VNPacket *vnp)
Definition: vn200_serial.c:102
euler angles
uint16_t ins_status
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:95
struct FloatVect3 vel_body
The estimated velocity in the imu frame, given in m/s.
Definition: vn200_serial.h:98
float theta
in radians
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
uint64_t nanostamp
Definition: vn200_serial.h:85
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
unsigned long long uint64_t
Definition: types.h:20
struct VectornavData vn_data
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
uint16_t calc_chk
Definition: vn200_serial.h:73
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
Vectornav VN-200 INS subsystem.
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
static unsigned short calculateCRC(unsigned char data[], unsigned int length)
Calculates the 16-bit CRC for the given ASCII or binary message.
Definition: vn200_serial.c:70
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
uint16_t datalength
Definition: vn200_serial.h:69
#define VN_GROUP_BYTES
Definition: vn200_serial.h:47
unsigned char uint8_t
Definition: types.h:14
static bool verify_chk(unsigned char data[], unsigned int length, uint16_t *calc_chk, uint16_t *rec_chk)
Verify checksum.
Definition: vn200_serial.c:88
float vel_u
NED velocity uncertainty [m/s].
Definition: vn200_serial.h:92
uint8_t msg_buf[VN_BUFFER_SIZE]
Definition: vn200_serial.h:66
#define VN_PAYLOAD_SIZE
Definition: vn200_serial.h:51
double pos_lla[3]
Definition: vn200_serial.h:102
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
#define VN_OUTPUT_GROUP
Definition: vn200_serial.h:46
uint64_t tow
tow (in nanoseconds), uint64
Definition: vn200_serial.h:99
enum VNMsgStatus status
Definition: vn200_serial.h:67
void vn200_check_status(struct VNData *vn_data)
Check INS status.
Definition: vn200_serial.c:39