include_v0.9/mavlink_helpers.h
Go to the documentation of this file.
1 #ifndef _MAVLINK_HELPERS_H_
2 #define _MAVLINK_HELPERS_H_
3 
4 #include "string.h"
5 #include "checksum.h"
6 #include "mavlink_types.h"
7 
8 #ifndef MAVLINK_HELPER
9 #define MAVLINK_HELPER
10 #endif
11 
12 /*
13  internal function to give access to the channel status for each channel
14  */
16 {
17  static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
18  return &m_mavlink_status[chan];
19 }
20 
21 /*
22  internal function to give access to the channel buffer for each channel
23  */
25 {
26 
27 #if MAVLINK_EXTERNAL_RX_BUFFER
28  // No m_mavlink_message array defined in function,
29  // has to be defined externally
30 #ifndef m_mavlink_message
31 #error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];)
32 #endif
33 #else
34  static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
35 #endif
36  return &m_mavlink_buffer[chan];
37 }
38 
51 #if MAVLINK_CRC_EXTRA
52 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
53  uint8_t chan, uint8_t length, uint8_t crc_extra)
54 #else
55 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
56  uint8_t chan, uint8_t length)
57 #endif
58 {
59  // This code part is the same for all messages;
60  uint16_t checksum;
61  msg->magic = MAVLINK_STX;
62  msg->len = length;
63  msg->sysid = system_id;
64  msg->compid = component_id;
65  // One sequence number per channel
68  checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
69 #if MAVLINK_CRC_EXTRA
70  crc_accumulate(crc_extra, &checksum);
71 #endif
72  mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
73  mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
74 
75  return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
76 }
77 
78 
82 #if MAVLINK_CRC_EXTRA
83 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
84  uint8_t length, uint8_t crc_extra)
85 {
86  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
87 }
88 #else
89 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
90  uint8_t length)
91 {
92  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
93 }
94 #endif
95 
96 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
97 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
98 
102 #if MAVLINK_CRC_EXTRA
103 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
104  uint8_t length, uint8_t crc_extra)
105 #else
106 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
107 #endif
108 {
109  uint16_t checksum;
110  uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
111  uint8_t ck[2];
113  buf[0] = MAVLINK_STX;
114  buf[1] = length;
115  buf[2] = status->current_tx_seq;
116  buf[3] = mavlink_system.sysid;
117  buf[4] = mavlink_system.compid;
118  buf[5] = msgid;
119  status->current_tx_seq++;
120  checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
121  crc_accumulate_buffer(&checksum, packet, length);
122 #if MAVLINK_CRC_EXTRA
123  crc_accumulate(crc_extra, &checksum);
124 #endif
125  ck[0] = (uint8_t)(checksum & 0xFF);
126  ck[1] = (uint8_t)(checksum >> 8);
127 
128  MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
129  _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
130  _mavlink_send_uart(chan, packet, length);
131  _mavlink_send_uart(chan, (const char *)ck, 2);
132  MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
133 }
134 
139 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
140 {
141  uint8_t ck[2];
142 
143  ck[0] = (uint8_t)(msg->checksum & 0xFF);
144  ck[1] = (uint8_t)(msg->checksum >> 8);
145 
147  _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
148  _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
149  _mavlink_send_uart(chan, (const char *)ck, 2);
151 }
152 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
153 
157 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
158 {
159  memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
160  return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
161 }
162 
164  uint8_t uint8;
165  int8_t int8;
166  uint16_t uint16;
167  int16_t int16;
168  uint32_t uint32;
169  int32_t int32;
170 };
171 
172 
174 {
175  crc_init(&msg->checksum);
176 }
177 
179 {
180  crc_accumulate(c, &msg->checksum);
181 }
182 
219 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
220 {
221  /*
222  default message crc function. You can override this per-system to
223  put this data in a different memory segment
224  */
225 #if MAVLINK_CRC_EXTRA
226 #ifndef MAVLINK_MESSAGE_CRC
227  static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
228 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
229 #endif
230 #endif
231 
232 
233 /* Enable this option to check the length of each message.
234 This allows invalid messages to be caught much sooner. Use if the transmission
235 medium is prone to missing (or extra) characters (e.g. a radio that fades in
236 and out). Only use if the channel will only contain messages types listed in
237 the headers.
238 */
239 #if MAVLINK_CHECK_MESSAGE_LENGTH
240 #ifndef MAVLINK_MESSAGE_LENGTH
241  static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
242 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
243 #endif
244 #endif
245 
248  int bufferIndex = 0;
249 
250  status->msg_received = 0;
251 
252  switch (status->parse_state)
253  {
256  if (c == MAVLINK_STX)
257  {
259  rxmsg->len = 0;
260  rxmsg->magic = c;
261  mavlink_start_checksum(rxmsg);
262  }
263  break;
264 
266  if (status->msg_received
267 /* Support shorter buffers than the
268  default maximum packet size */
269 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
271 #endif
272  )
273  {
274  status->buffer_overrun++;
275  status->parse_error++;
276  status->msg_received = 0;
278  }
279  else
280  {
281  // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
282  rxmsg->len = c;
283  status->packet_idx = 0;
284  mavlink_update_checksum(rxmsg, c);
286  }
287  break;
288 
290  rxmsg->seq = c;
291  mavlink_update_checksum(rxmsg, c);
293  break;
294 
296  rxmsg->sysid = c;
297  mavlink_update_checksum(rxmsg, c);
299  break;
300 
302  rxmsg->compid = c;
303  mavlink_update_checksum(rxmsg, c);
305  break;
306 
308 #if MAVLINK_CHECK_MESSAGE_LENGTH
309  if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
310  {
311  status->parse_error++;
313  break;
314  }
315 #endif
316  rxmsg->msgid = c;
317  mavlink_update_checksum(rxmsg, c);
318  if (rxmsg->len == 0)
319  {
321  }
322  else
323  {
325  }
326  break;
327 
329  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
330  mavlink_update_checksum(rxmsg, c);
331  if (status->packet_idx == rxmsg->len)
332  {
334  }
335  break;
336 
338 #if MAVLINK_CRC_EXTRA
340 #endif
341  if (c != (rxmsg->checksum & 0xFF)) {
342  // Check first checksum byte
343  status->parse_error++;
344  status->msg_received = 0;
346  if (c == MAVLINK_STX)
347  {
349  rxmsg->len = 0;
350  mavlink_start_checksum(rxmsg);
351  }
352  }
353  else
354  {
356  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
357  }
358  break;
359 
361  if (c != (rxmsg->checksum >> 8)) {
362  // Check second checksum byte
363  status->parse_error++;
364  status->msg_received = 0;
366  if (c == MAVLINK_STX)
367  {
369  rxmsg->len = 0;
370  mavlink_start_checksum(rxmsg);
371  }
372  }
373  else
374  {
375  // Successfully got message
376  status->msg_received = 1;
378  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
379  memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
380  }
381  break;
382  }
383 
384  bufferIndex++;
385  // If a message has been sucessfully decoded, check index
386  if (status->msg_received == 1)
387  {
388  //while(status->current_seq != rxmsg->seq)
389  //{
390  // status->packet_rx_drop_count++;
391  // status->current_seq++;
392  //}
393  status->current_rx_seq = rxmsg->seq;
394  // Initial condition: If no packet has been received so far, drop count is undefined
395  if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
396  // Count this packet as received
397  status->packet_rx_success_count++;
398  }
399 
400  r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
401  r_mavlink_status->parse_state = status->parse_state;
402  r_mavlink_status->packet_idx = status->packet_idx;
403  r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
404  r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
405  r_mavlink_status->packet_rx_drop_count = status->parse_error;
406  status->parse_error = 0;
407  return status->msg_received;
408 }
409 
420 MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
421 {
422  uint16_t bits_remain = bits;
423  // Transform number into network order
424  int32_t v;
425  uint8_t i_bit_index, i_byte_index, curr_bits_n;
426 #if MAVLINK_NEED_BYTE_SWAP
427  union {
428  int32_t i;
429  uint8_t b[4];
430  } bin, bout;
431  bin.i = b;
432  bout.b[0] = bin.b[3];
433  bout.b[1] = bin.b[2];
434  bout.b[2] = bin.b[1];
435  bout.b[3] = bin.b[0];
436  v = bout.i;
437 #else
438  v = b;
439 #endif
440 
441  // buffer in
442  // 01100000 01000000 00000000 11110001
443  // buffer out
444  // 11110001 00000000 01000000 01100000
445 
446  // Existing partly filled byte (four free slots)
447  // 0111xxxx
448 
449  // Mask n free bits
450  // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
451  // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
452 
453  // Shift n bits into the right position
454  // out = in >> n;
455 
456  // Mask and shift bytes
457  i_bit_index = bit_index;
458  i_byte_index = packet_index;
459  if (bit_index > 0)
460  {
461  // If bits were available at start, they were available
462  // in the byte before the current index
463  i_byte_index--;
464  }
465 
466  // While bits have not been packed yet
467  while (bits_remain > 0)
468  {
469  // Bits still have to be packed
470  // there can be more than 8 bits, so
471  // we might have to pack them into more than one byte
472 
473  // First pack everything we can into the current 'open' byte
474  //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
475  //FIXME
476  if (bits_remain <= (uint8_t)(8 - i_bit_index))
477  {
478  // Enough space
479  curr_bits_n = (uint8_t)bits_remain;
480  }
481  else
482  {
483  curr_bits_n = (8 - i_bit_index);
484  }
485 
486  // Pack these n bits into the current byte
487  // Mask out whatever was at that position with ones (xxx11111)
488  buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
489  // Put content to this position, by masking out the non-used part
490  buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
491 
492  // Increment the bit index
493  i_bit_index += curr_bits_n;
494 
495  // Now proceed to the next byte, if necessary
496  bits_remain -= curr_bits_n;
497  if (bits_remain > 0)
498  {
499  // Offer another 8 bits / one byte
500  i_byte_index++;
501  i_bit_index = 0;
502  }
503  }
504 
505  *r_bit_index = i_bit_index;
506  // If a partly filled byte is present, mark this as consumed
507  if (i_bit_index != 7) i_byte_index++;
508  return i_byte_index - packet_index;
509 }
510 
511 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
512 
513 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
514 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
515 // whole packet at a time
516 
517 /*
518 
519 #include "mavlink_types.h"
520 
521 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
522 {
523  if (chan == MAVLINK_COMM_0)
524  {
525  uart0_transmit(ch);
526  }
527  if (chan == MAVLINK_COMM_1)
528  {
529  uart1_transmit(ch);
530  }
531 }
532  */
533 
534 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
535 {
536 #ifdef MAVLINK_SEND_UART_BYTES
537  /* this is the more efficient approach, if the platform
538  defines it */
539  MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len);
540 #else
541  /* fallback to one byte at a time */
542  uint16_t i;
543  for (i = 0; i < len; i++) {
544  comm_send_ch(chan, (uint8_t)buf[i]);
545  }
546 #endif
547 }
548 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
549 
550 #endif /* _MAVLINK_HELPERS_H_ */
#define MAVLINK_MESSAGE_CRCS
static void crc_init(uint16_t *crcAccum)
Initiliaze the buffer for the X.25 CRC.
#define MAVLINK_MESSAGE_LENGTH(msgid)
Definition: mavnative.c:64
#define MAVLINK_MESSAGE_CRC(msgid)
Definition: mavnative.c:55
#define MAVLINK_MESSAGE_LENGTHS
static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
Definition: testmav.c:135
#define MAVLINK_START_UART_SEND(chan, length)
static uint16_t crc_calculate(const uint8_t *pBuffer, uint16_t length)
Calculates the X.25 checksum on a byte buffer.
#define MAVLINK_END_UART_SEND(chan, length)
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
static mavlink_system_t mavlink_system
Definition: testmav.c:21
static void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
Accumulate the X.25 CRC by adding an array of bytes.
static void crc_accumulate(uint8_t data, uint16_t *crcAccum)
Accumulate the X.25 CRC by adding one char at a time.


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07