Paparazzi UAS  v5.15_devel-99-g2ff7410
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
mavlink_decoder.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Xavier Gibert
3  * Copyright (C) 2013 Gautier Hattenberger
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  *
22  */
23 
28 #ifndef MAVLINK_DECODER_H
29 #define MAVLINK_DECODER_H
30 
31 #include "std.h"
32 #include "pprzlink/pprzlink_transport.h"
33 #include "mcu_periph/uart.h"
34 
35 /* MAVLINK Transport
36  */
37 
38 #define STXMAV 0xFE
39 
54 };
55 
56 #define MAVLINK_PAYLOAD_OFFSET 4
57 
58 #define MAVLINK_SEQ_IDX 0
59 #define MAVLINK_SYS_ID_IDX 1
60 #define MAVLINK_COMP_ID_IDX 2
61 #define MAVLINK_MSG_ID_IDX 3
62 
64 #define X25_INIT_CRC 0xffff
65 
66 #ifndef MAVLINK_NO_CRC_EXTRA
67 // CRC Extra (!!! staticaly calculated !!!)
68 extern uint8_t mavlink_crc_extra[256];
69 #endif
70 
80 static inline void mavlink_crc_accumulate(uint8_t data, uint16_t *crcAccum)
81 {
82  /*Accumulate one byte of data into the CRC*/
83  uint8_t tmp;
84 
85  tmp = data ^ (uint8_t)(*crcAccum & 0xff);
86  tmp ^= (tmp << 4);
87  *crcAccum = (*crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4);
88 }
89 
95 static inline void mavlink_crc_init(uint16_t *crcAccum)
96 {
97  *crcAccum = X25_INIT_CRC;
98 }
99 
107 static inline uint16_t mavlink_crc_calculate(const uint8_t *pBuffer, uint16_t length)
108 {
109  uint16_t crcTmp;
110  mavlink_crc_init(&crcTmp);
111  while (length--) {
112  mavlink_crc_accumulate(*pBuffer++, &crcTmp);
113  }
114  return crcTmp;
115 }
116 
119 // Mavlink parsing state machine
120 typedef enum {
128 
134  void (*callback)(struct mavlink_message *msg);
136 };
137 
141  // generic interface
142  struct transport_rx trans;
143  // specific mavlink transport variables
147  // linked list of callbacks
149 };
150 
151 extern struct mavlink_transport mavlink_tp;
152 
153 #if MAVLINK_DECODER_DEBUG
154 
158 extern void mavlink_send_debug(struct mavlink_transport *t);
159 #endif
160 
163 static inline void mavlink_register_msg(struct mavlink_transport *t, struct mavlink_msg_req *req)
164 {
165  // handle linked list of requests
166  req->next = t->req;
167  t->req = req;
168 }
169 
172 static inline void parse_mavlink(struct mavlink_transport *t, uint8_t c)
173 {
174  switch (t->status) {
176  t->status = MAVLINK_PARSE_STATE_IDLE; // directly go to idle state (no break)
178  if (c == STXMAV) {
180  mavlink_crc_init(&(t->checksum));
181  }
182  break;
184  if (t->trans.msg_received) {
185  t->trans.ovrn++;
186  goto error;
187  }
188  t->trans.payload_len = c +
189  MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */
192  t->payload_idx = 0;
193  break;
195  t->trans.payload[t->payload_idx] = c;
197  t->payload_idx++;
198  if (t->payload_idx == t->trans.payload_len) {
200  }
201  break;
203 #if MAVLINK_DECODER_DEBUG
204  mavlink_send_debug(t);
205 #endif
206 #ifndef MAVLINK_NO_CRC_EXTRA
207  // add extra CRC
209 #endif
210  if (c != (t->checksum & 0xFF)) {
211  goto error;
212  }
214  break;
216  if (c != (t->checksum >> 8)) {
217  goto error;
218  }
219  t->trans.msg_received = true;
220  goto restart;
221  default:
222  goto error;
223  }
224  return;
225 error:
226  t->trans.error++;
227 restart:
229  return;
230 }
231 
232 static inline void mavlink_parse_payload(struct mavlink_transport *t)
233 {
234  uint8_t i;
235  struct mavlink_msg_req *el;
236  // test the linked list and call callback if needed
237  for (el = t->req; el; el = el->next) {
238  if (el->msg_id == t->trans.payload[MAVLINK_MSG_ID_IDX]) {
239  // build message out of payload
240  el->msg.seq = t->trans.payload[MAVLINK_SEQ_IDX];
241  el->msg.sys_id = t->trans.payload[MAVLINK_SYS_ID_IDX];
242  el->msg.comp_id = t->trans.payload[MAVLINK_COMP_ID_IDX];
243  el->msg.msg_id = t->trans.payload[MAVLINK_MSG_ID_IDX];
244  // copy buffer
245  for (i = 0; i < t->trans.payload_len; i++) {
246  el->msg.payload[i] = t->trans.payload[i + MAVLINK_PAYLOAD_OFFSET];
247  }
248  // callback
249  el->callback(&(el->msg));
250  }
251  }
252 }
253 
254 static inline void mavlink_check_and_parse(struct link_device *dev, struct mavlink_transport *trans)
255 {
256  if (dev->char_available(dev->periph)) {
257  while (dev->char_available(dev->periph) && !trans->trans.msg_received) {
258  parse_mavlink(trans, dev->get_byte(dev->periph));
259  }
260  if (trans->trans.msg_received) {
261  mavlink_parse_payload(trans);
262  trans->trans.msg_received = false;
263  }
264  }
265 }
266 
267 /* Datalink Event Macro */
268 #define MavlinkDatalinkEvent() mavlink_check_and_parse(&(MAVLINK_UART).device, &mavlink_tp)
269 
270 #endif /* MAVLINK_DECODER_H */
271 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
unsigned char uint8_t
Definition: types.h:14