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
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 
37 static inline unsigned short calculateCRC(unsigned char data[], unsigned int length)
38 {
39  unsigned int i;
40  unsigned short crc = 0;
41  for (i = 0; i < length; i++) {
42  crc = (unsigned char)(crc >> 8) | (crc << 8);
43  crc ^= data[i];
44  crc ^= (unsigned char)(crc & 0xff) >> 4;
45  crc ^= crc << 12;
46  crc ^= (crc & 0x00ff) << 5;
47  }
48  return crc;
49 }
50 
51 
55 static inline bool verify_chk(unsigned char data[], unsigned int length, uint16_t *calc_chk, uint16_t *rec_chk)
56 {
57  unsigned short calc_crc = calculateCRC(data, length);
58  unsigned short rec_crc = (unsigned short)(data[length] << 8 | data[length + 1]);
59  *calc_chk = (uint16_t) calc_crc;
60  *rec_chk = (uint16_t) rec_crc;
61  if (calc_crc == rec_crc) {
62  return 1;
63  } else {
64  return 0;
65  }
66 }
67 
68 
69 static inline void vn200_read_buffer(struct VNPacket *vnp)
70 {
71  while (uart_char_available(&VN_PORT) && !(vnp->msg_available)) {
72  vn200_parse(vnp, uart_getch(&VN_PORT));
73  }
74 }
75 
76 
77 void vn200_event(struct VNPacket *vnp)
78 {
79  if (uart_char_available(&VN_PORT)) {
80  vn200_read_buffer(vnp);
81  }
82 }
83 
87 void vn200_parse(struct VNPacket *vnp, uint8_t c)
88 {
89  switch (vnp->status) {
90  case VNMsgSync:
91  // sync the header
92  vnp->msg_idx = 0;
93  if (c == VN_SYNC) {
94  vnp->status = VNMsgHeader;
95  } else {
96  vnp->hdr_error++;
97  }
98  break;
99  case VNMsgHeader:
100  // read header data (we expect 0x39)
101  if (c == VN_OUTPUT_GROUP) {
102  // increment idx and save current byte for checksum
103  vnp->status = VNMsgGroup;
104  vnp->msg_buf[vnp->msg_idx] = c;
105  vnp->msg_idx++;
106  } else {
107  vnp->hdr_error++;
108  vnp->status = VNMsgSync;
109  }
110  break;
111  break;
112  case VNMsgGroup:
113  // read header data
114  vnp->msg_buf[vnp->msg_idx] = c;
115  vnp->msg_idx++;
116  if (vnp->msg_idx == VN_GROUP_BYTES) {
118  vnp->status = VNMsgData;
119  }
120  break;
121  case VNMsgData:
122  vnp->msg_buf[vnp->msg_idx] = c;
123  vnp->msg_idx++;
124  if (vnp->msg_idx == (vnp->datalength + 2)) {
125  if (verify_chk(vnp->msg_buf, vnp->datalength, &(vnp->calc_chk), &(vnp->rec_chk))) {
126  vnp->msg_available = true;
127  vnp->counter++;
128  } else {
129  vnp->msg_available = false;
130  vnp->chksm_error++;
131  }
132  vnp->status = VNMsgSync;
133  }
134  break;
135  default:
136  vnp->status = VNMsgSync;
137  vnp->msg_idx = 0;
138  break;
139  }
140 }
unsigned short uint16_t
Definition: types.h:16
#define VN_HEADER_SIZE
Definition: vn200_serial.h:43
#define VN_SYNC
Definition: vn200_serial.h:38
uint8_t msg_idx
Definition: vn200_serial.h:61
uint16_t rec_chk
Definition: vn200_serial.h:67
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:822
uint32_t chksm_error
Definition: vn200_serial.h:57
uint16_t counter
Definition: vn200_serial.h:68
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:296
uint32_t hdr_error
Definition: vn200_serial.h:58
bool msg_available
Definition: vn200_serial.h:56
void vn200_parse(struct VNPacket *vnp, uint8_t c)
Packet Collection & state machine.
Definition: vn200_serial.c:87
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:77
static void vn200_read_buffer(struct VNPacket *vnp)
Definition: vn200_serial.c:69
uint16_t calc_chk
Definition: vn200_serial.h:66
Vectornav VN-200 INS subsystem.
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:37
uint16_t datalength
Definition: vn200_serial.h:62
#define VN_GROUP_BYTES
Definition: vn200_serial.h:40
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:55
uint8_t msg_buf[VN_BUFFER_SIZE]
Definition: vn200_serial.h:59
#define VN_PAYLOAD_SIZE
Definition: vn200_serial.h:44
#define VN_OUTPUT_GROUP
Definition: vn200_serial.h:39
enum VNMsgStatus status
Definition: vn200_serial.h:60