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 #include "mavlink_conversions.h"
8 
9 #ifndef MAVLINK_HELPER
10 #define MAVLINK_HELPER
11 #endif
12 
13 /*
14  * Internal function to give access to the channel status for each channel
15  */
16 #ifndef MAVLINK_GET_CHANNEL_STATUS
18 {
19 #ifdef MAVLINK_EXTERNAL_RX_STATUS
20  // No m_mavlink_status array defined in function,
21  // has to be defined externally
22 #else
23  static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
24 #endif
25  return &m_mavlink_status[chan];
26 }
27 #endif
28 
29 /*
30  * Internal function to give access to the channel buffer for each channel
31  */
32 #ifndef MAVLINK_GET_CHANNEL_BUFFER
33 MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
34 {
35 
36 #ifdef MAVLINK_EXTERNAL_RX_BUFFER
37  // No m_mavlink_buffer array defined in function,
38  // has to be defined externally
39 #else
40  static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
41 #endif
42  return &m_mavlink_buffer[chan];
43 }
44 #endif
45 
50 {
53 }
54 
67 #if MAVLINK_CRC_EXTRA
68 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
69  uint8_t chan, uint8_t length, uint8_t crc_extra)
70 #else
71 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
72  uint8_t chan, uint8_t length)
73 #endif
74 {
75  // This code part is the same for all messages;
76  msg->magic = MAVLINK_STX;
77  msg->len = length;
78  msg->sysid = system_id;
79  msg->compid = component_id;
80  // One sequence number per channel
83  msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
84  crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
85 #if MAVLINK_CRC_EXTRA
86  crc_accumulate(crc_extra, &msg->checksum);
87 #endif
88  mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
89  mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
90 
91  return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
92 }
93 
94 
98 #if MAVLINK_CRC_EXTRA
99 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
100  uint8_t length, uint8_t crc_extra)
101 {
102  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
103 }
104 #else
105 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
106  uint8_t length)
107 {
108  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
109 }
110 #endif
111 
112 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
113 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
114 
118 #if MAVLINK_CRC_EXTRA
119 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
120  uint8_t length, uint8_t crc_extra)
121 #else
122 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
123 #endif
124 {
125  uint16_t checksum;
126  uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
127  uint8_t ck[2];
129  buf[0] = MAVLINK_STX;
130  buf[1] = length;
131  buf[2] = status->current_tx_seq;
132  buf[3] = mavlink_system.sysid;
133  buf[4] = mavlink_system.compid;
134  buf[5] = msgid;
135  status->current_tx_seq++;
136  checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
137  crc_accumulate_buffer(&checksum, packet, length);
138 #if MAVLINK_CRC_EXTRA
139  crc_accumulate(crc_extra, &checksum);
140 #endif
141  ck[0] = (uint8_t)(checksum & 0xFF);
142  ck[1] = (uint8_t)(checksum >> 8);
143 
144  MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
145  _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
146  _mavlink_send_uart(chan, packet, length);
147  _mavlink_send_uart(chan, (const char *)ck, 2);
148  MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
149 }
150 
155 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
156 {
157  uint8_t ck[2];
158 
159  ck[0] = (uint8_t)(msg->checksum & 0xFF);
160  ck[1] = (uint8_t)(msg->checksum >> 8);
161  // XXX use the right sequence here
162 
164  _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
165  _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
166  _mavlink_send_uart(chan, (const char *)ck, 2);
168 }
169 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
170 
174 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
175 {
176  memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
177 
178  uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
179 
180  ck[0] = (uint8_t)(msg->checksum & 0xFF);
181  ck[1] = (uint8_t)(msg->checksum >> 8);
182 
183  return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
184 }
185 
187  uint8_t uint8;
188  int8_t int8;
189  uint16_t uint16;
190  int16_t int16;
191  uint32_t uint32;
192  int32_t int32;
193 };
194 
195 
196 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
197 {
198  crc_init(&msg->checksum);
199 }
200 
201 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
202 {
203  crc_accumulate(c, &msg->checksum);
204 }
205 
240 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
241  mavlink_status_t* status,
242  uint8_t c,
243  mavlink_message_t* r_message,
244  mavlink_status_t* r_mavlink_status)
245 {
246  /*
247  default message crc function. You can override this per-system to
248  put this data in a different memory segment
249  */
250 #if MAVLINK_CRC_EXTRA
251 #ifndef MAVLINK_MESSAGE_CRC
252  static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
253 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
254 #endif
255 #endif
256 
257  /* Enable this option to check the length of each message.
258  This allows invalid messages to be caught much sooner. Use if the transmission
259  medium is prone to missing (or extra) characters (e.g. a radio that fades in
260  and out). Only use if the channel will only contain messages types listed in
261  the headers.
262  */
263 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
264 #ifndef MAVLINK_MESSAGE_LENGTH
265  static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
266 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
267 #endif
268 #endif
269 
270  int bufferIndex = 0;
271 
273 
274  switch (status->parse_state)
275  {
278  if (c == MAVLINK_STX)
279  {
281  rxmsg->len = 0;
282  rxmsg->magic = c;
283  mavlink_start_checksum(rxmsg);
284  }
285  break;
286 
288  if (status->msg_received
289 /* Support shorter buffers than the
290  default maximum packet size */
291 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
293 #endif
294  )
295  {
296  status->buffer_overrun++;
297  status->parse_error++;
298  status->msg_received = 0;
300  }
301  else
302  {
303  // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
304  rxmsg->len = c;
305  status->packet_idx = 0;
306  mavlink_update_checksum(rxmsg, c);
308  }
309  break;
310 
312  rxmsg->seq = c;
313  mavlink_update_checksum(rxmsg, c);
315  break;
316 
318  rxmsg->sysid = c;
319  mavlink_update_checksum(rxmsg, c);
321  break;
322 
324  rxmsg->compid = c;
325  mavlink_update_checksum(rxmsg, c);
327  break;
328 
330 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
331  if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
332  {
333  status->parse_error++;
335  break;
336  }
337 #endif
338  rxmsg->msgid = c;
339  mavlink_update_checksum(rxmsg, c);
340  if (rxmsg->len == 0)
341  {
343  }
344  else
345  {
347  }
348  break;
349 
351  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
352  mavlink_update_checksum(rxmsg, c);
353  if (status->packet_idx == rxmsg->len)
354  {
356  }
357  break;
358 
360 #if MAVLINK_CRC_EXTRA
361  mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
362 #endif
363  if (c != (rxmsg->checksum & 0xFF)) {
365  } else {
367  }
368  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
369  break;
370 
373  if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
374  // got a bad CRC message
376  } else {
377  // Successfully got message
379  }
381  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
382  memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
383  break;
384  }
385 
386  bufferIndex++;
387  // If a message has been sucessfully decoded, check index
388  if (status->msg_received == MAVLINK_FRAMING_OK)
389  {
390  //while(status->current_seq != rxmsg->seq)
391  //{
392  // status->packet_rx_drop_count++;
393  // status->current_seq++;
394  //}
395  status->current_rx_seq = rxmsg->seq;
396  // Initial condition: If no packet has been received so far, drop count is undefined
397  if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
398  // Count this packet as received
399  status->packet_rx_success_count++;
400  }
401 
402  r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
403  r_mavlink_status->parse_state = status->parse_state;
404  r_mavlink_status->packet_idx = status->packet_idx;
405  r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
406  r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
407  r_mavlink_status->packet_rx_drop_count = status->parse_error;
408  status->parse_error = 0;
409 
410  if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
411  /*
412  the CRC came out wrong. We now need to overwrite the
413  msg CRC with the one on the wire so that if the
414  caller decides to forward the message anyway that
415  mavlink_msg_to_send_buffer() won't overwrite the
416  checksum
417  */
418  r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
419  }
420 
421  return status->msg_received;
422 }
423 
465 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
466 {
469  c,
470  r_message,
471  r_mavlink_status);
472 }
473 
474 
515 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
516 {
517  uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
518  if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
519  // we got a bad CRC. Treat as a parse failure
520  mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
522  status->parse_error++;
525  if (c == MAVLINK_STX)
526  {
528  rxmsg->len = 0;
529  mavlink_start_checksum(rxmsg);
530  }
531  return 0;
532  }
533  return msg_received;
534 }
535 
546 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)
547 {
548  uint16_t bits_remain = bits;
549  // Transform number into network order
550  int32_t v;
551  uint8_t i_bit_index, i_byte_index, curr_bits_n;
552 #if MAVLINK_NEED_BYTE_SWAP
553  union {
554  int32_t i;
555  uint8_t b[4];
556  } bin, bout;
557  bin.i = b;
558  bout.b[0] = bin.b[3];
559  bout.b[1] = bin.b[2];
560  bout.b[2] = bin.b[1];
561  bout.b[3] = bin.b[0];
562  v = bout.i;
563 #else
564  v = b;
565 #endif
566 
567  // buffer in
568  // 01100000 01000000 00000000 11110001
569  // buffer out
570  // 11110001 00000000 01000000 01100000
571 
572  // Existing partly filled byte (four free slots)
573  // 0111xxxx
574 
575  // Mask n free bits
576  // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
577  // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
578 
579  // Shift n bits into the right position
580  // out = in >> n;
581 
582  // Mask and shift bytes
583  i_bit_index = bit_index;
584  i_byte_index = packet_index;
585  if (bit_index > 0)
586  {
587  // If bits were available at start, they were available
588  // in the byte before the current index
589  i_byte_index--;
590  }
591 
592  // While bits have not been packed yet
593  while (bits_remain > 0)
594  {
595  // Bits still have to be packed
596  // there can be more than 8 bits, so
597  // we might have to pack them into more than one byte
598 
599  // First pack everything we can into the current 'open' byte
600  //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
601  //FIXME
602  if (bits_remain <= (uint8_t)(8 - i_bit_index))
603  {
604  // Enough space
605  curr_bits_n = (uint8_t)bits_remain;
606  }
607  else
608  {
609  curr_bits_n = (8 - i_bit_index);
610  }
611 
612  // Pack these n bits into the current byte
613  // Mask out whatever was at that position with ones (xxx11111)
614  buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
615  // Put content to this position, by masking out the non-used part
616  buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
617 
618  // Increment the bit index
619  i_bit_index += curr_bits_n;
620 
621  // Now proceed to the next byte, if necessary
622  bits_remain -= curr_bits_n;
623  if (bits_remain > 0)
624  {
625  // Offer another 8 bits / one byte
626  i_byte_index++;
627  i_bit_index = 0;
628  }
629  }
630 
631  *r_bit_index = i_bit_index;
632  // If a partly filled byte is present, mark this as consumed
633  if (i_bit_index != 7) i_byte_index++;
634  return i_byte_index - packet_index;
635 }
636 
637 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
638 
639 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
640 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
641 // whole packet at a time
642 
643 /*
644 
645 #include "mavlink_types.h"
646 
647 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
648 {
649  if (chan == MAVLINK_COMM_0)
650  {
651  uart0_transmit(ch);
652  }
653  if (chan == MAVLINK_COMM_1)
654  {
655  uart1_transmit(ch);
656  }
657 }
658  */
659 
660 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
661 {
662 #ifdef MAVLINK_SEND_UART_BYTES
663  /* this is the more efficient approach, if the platform
664  defines it */
665  MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
666 #else
667  /* fallback to one byte at a time */
668  uint16_t i;
669  for (i = 0; i < len; i++) {
670  comm_send_ch(chan, (uint8_t)buf[i]);
671  }
672 #endif
673 }
674 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
675 
676 #endif /* _MAVLINK_HELPERS_H_ */
#define MAVLINK_MESSAGE_CRCS
Definition: common.h:23
static uint16_t crc_calculate(const uint8_t *pBuffer, uint16_t length)
Calculates the X.25 checksum on a byte buffer.
Definition: checksum.h:64
static void crc_accumulate(uint8_t data, uint16_t *crcAccum)
Accumulate the X.25 CRC by adding one char at a time.
Definition: checksum.h:34
#define MAVLINK_START_UART_SEND(chan, length)
Definition: protocol.h:30
#define MAVLINK_MESSAGE_LENGTHS
Definition: common.h:19
static void crc_init(uint16_t *crcAccum)
Initiliaze the buffer for the X.25 CRC.
Definition: checksum.h:51
static void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
Accumulate the X.25 CRC by adding an array of bytes.
Definition: checksum.h:84
#define MAVLINK_END_UART_SEND(chan, length)
Definition: protocol.h:34
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)


rosflight
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 20:00:12