Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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"
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 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  // handle linked list of requests
165  req->next = t->req;
166  t->req = req;
167 }
168 
171 static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) {
172  switch (t->status) {
174  t->status = MAVLINK_PARSE_STATE_IDLE; // directly go to idle state (no break)
176  if (c == STXMAV) {
178  mavlink_crc_init(&(t->checksum));
179  }
180  break;
182  if (t->trans.msg_received) {
183  t->trans.ovrn++;
184  goto error;
185  }
186  t->trans.payload_len = c + MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */
189  t->payload_idx = 0;
190  break;
192  t->trans.payload[t->payload_idx] = c;
194  t->payload_idx++;
195  if (t->payload_idx == t->trans.payload_len)
197  break;
199 #if MAVLINK_DECODER_DEBUG
200  mavlink_send_debug(t);
201 #endif
202 #ifndef MAVLINK_NO_CRC_EXTRA
203  // add extra CRC
205 #endif
206  if (c != (t->checksum & 0xFF))
207  goto error;
209  break;
211  if (c != (t->checksum >> 8))
212  goto error;
213  t->trans.msg_received = TRUE;
214  goto restart;
215  default:
216  goto error;
217  }
218  return;
219 error:
220  t->trans.error++;
221 restart:
223  return;
224 }
225 
226 static inline void mavlink_parse_payload(struct mavlink_transport * t) {
227  uint8_t i;
228  struct mavlink_msg_req * el;
229  // test the linked list and call callback if needed
230  for (el = t->req; el; el = el->next) {
231  if (el->msg_id == t->trans.payload[MAVLINK_MSG_ID_IDX]) {
232  // build message out of payload
233  el->msg.seq = t->trans.payload[MAVLINK_SEQ_IDX];
237  // copy buffer
238  for(i = 0; i < t->trans.payload_len; i++)
240  // callback
241  el->callback(&(el->msg));
242  }
243  }
244 }
245 
246 
247 #define MavlinkBuffer(_dev) TransportLink(_dev,ChAvailable())
248 #define ReadMavlinkBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_mavlink(&(_trans),TransportLink(_dev,Getch())); }
249 
250 #define MavlinkCheckAndParse(_dev,_trans) { \
251  if (MavlinkBuffer(_dev)) { \
252  ReadMavlinkBuffer(_dev,_trans); \
253  if (_trans.trans.msg_received) { \
254  mavlink_parse_payload(&(_trans)); \
255  _trans.trans.msg_received = FALSE; \
256  } \
257  } \
258 }
259 
260 /* Datalink Event Macro */
261 #define MavlinkDatalinkEvent() MavlinkCheckAndParse(MAVLINK_UART, mavlink_tp)
262 
263 #endif /* MAVLINK_DECODER_H */
264 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
generic transport header
Generic transport header.
Definition: transport.h:39
volatile uint8_t payload_len
Definition: transport.h:43
uint8_t payload[TRANSPORT_PAYLOAD_LEN]
Definition: transport.h:41
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
volatile bool_t msg_received
Definition: transport.h:45
struct mavlink_msg_req req
Definition: px4flow.c:42
uint8_t ovrn
Definition: transport.h:47
uint8_t error
Definition: transport.h:47
static struct point c
Definition: discsurvey.c:39