include_v1.0/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
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 min_length, 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 is only used for the v2 protocol and we silence it here
76  (void)min_length;
77  // This code part is the same for all messages;
78  msg->magic = MAVLINK_STX;
79  msg->len = length;
80  msg->sysid = system_id;
81  msg->compid = component_id;
82  // One sequence number per channel
85  msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
87 #if MAVLINK_CRC_EXTRA
88  crc_accumulate(crc_extra, &msg->checksum);
89 #endif
90  mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
91  mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
92 
93  return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
94 }
95 
96 
100 #if MAVLINK_CRC_EXTRA
101 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
102  uint8_t min_length, uint8_t length, uint8_t crc_extra)
103 {
104  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
105 }
106 #else
107 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
108  uint8_t length)
109 {
110  return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
111 }
112 #endif
113 
114 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
115 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
116 
120 #if MAVLINK_CRC_EXTRA
121 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
122  uint8_t min_length, uint8_t length, uint8_t crc_extra)
123 #else
124 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
125 #endif
126 {
127  uint16_t checksum;
128  uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
129  uint8_t ck[2];
131  buf[0] = MAVLINK_STX;
132  buf[1] = length;
133  buf[2] = status->current_tx_seq;
134  buf[3] = mavlink_system.sysid;
135  buf[4] = mavlink_system.compid;
136  buf[5] = msgid;
137  status->current_tx_seq++;
138  checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
139  crc_accumulate_buffer(&checksum, packet, length);
140 #if MAVLINK_CRC_EXTRA
141  crc_accumulate(crc_extra, &checksum);
142 #endif
143  ck[0] = (uint8_t)(checksum & 0xFF);
144  ck[1] = (uint8_t)(checksum >> 8);
145 
146  MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
147  _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
148  _mavlink_send_uart(chan, packet, length);
149  _mavlink_send_uart(chan, (const char *)ck, 2);
150  MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
151 }
152 
157 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
158 {
159  uint8_t ck[2];
160 
161  ck[0] = (uint8_t)(msg->checksum & 0xFF);
162  ck[1] = (uint8_t)(msg->checksum >> 8);
163  // XXX use the right sequence here
164 
166  _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
167  _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
168  _mavlink_send_uart(chan, (const char *)ck, 2);
170 }
171 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
172 
176 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
177 {
178  memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
179 
180  uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
181 
182  ck[0] = (uint8_t)(msg->checksum & 0xFF);
183  ck[1] = (uint8_t)(msg->checksum >> 8);
184 
185  return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
186 }
187 
188 union __mavlink_bitfield {
189  uint8_t uint8;
190  int8_t int8;
191  uint16_t uint16;
192  int16_t int16;
193  uint32_t uint32;
194  int32_t int32;
195 };
196 
197 
199 {
200  crc_init(&msg->checksum);
201 }
202 
204 {
205  crc_accumulate(c, &msg->checksum);
206 }
207 
222  mavlink_status_t* status,
223  uint8_t c,
224  mavlink_message_t* r_message,
225  mavlink_status_t* r_mavlink_status)
226 {
227  /*
228  default message crc function. You can override this per-system to
229  put this data in a different memory segment
230  */
231 #if MAVLINK_CRC_EXTRA
232 #ifndef MAVLINK_MESSAGE_CRC
233  static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
234 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
235 #endif
236 #endif
237 
238  /* Enable this option to check the length of each message.
239  This allows invalid messages to be caught much sooner. Use if the transmission
240  medium is prone to missing (or extra) characters (e.g. a radio that fades in
241  and out). Only use if the channel will only contain messages types listed in
242  the headers.
243  */
244 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
245 #ifndef MAVLINK_MESSAGE_LENGTH
246  static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
247 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
248 #endif
249 #endif
250 
251  int bufferIndex = 0;
252 
254 
255  switch (status->parse_state)
256  {
259  if (c == MAVLINK_STX)
260  {
262  rxmsg->len = 0;
263  rxmsg->magic = c;
264  mavlink_start_checksum(rxmsg);
265  }
266  break;
267 
269  if (status->msg_received
270 /* Support shorter buffers than the
271  default maximum packet size */
272 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
274 #endif
275  )
276  {
277  status->buffer_overrun++;
278  status->parse_error++;
279  status->msg_received = 0;
281  }
282  else
283  {
284  // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
285  rxmsg->len = c;
286  status->packet_idx = 0;
287  mavlink_update_checksum(rxmsg, c);
289  }
290  break;
291 
293  rxmsg->seq = c;
294  mavlink_update_checksum(rxmsg, c);
296  break;
297 
299  rxmsg->sysid = c;
300  mavlink_update_checksum(rxmsg, c);
302  break;
303 
305  rxmsg->compid = c;
306  mavlink_update_checksum(rxmsg, c);
308  break;
309 
311 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
312  if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
313  {
314  status->parse_error++;
316  break;
317  }
318 #endif
319  rxmsg->msgid = c;
320  mavlink_update_checksum(rxmsg, c);
321  if (rxmsg->len == 0)
322  {
324  }
325  else
326  {
328  }
329  break;
330 
332  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
333  mavlink_update_checksum(rxmsg, c);
334  if (status->packet_idx == rxmsg->len)
335  {
337  }
338  break;
339 
341 #if MAVLINK_CRC_EXTRA
343 #endif
344  if (c != (rxmsg->checksum & 0xFF)) {
346  } else {
348  }
349  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
350  break;
351 
354  if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
355  // got a bad CRC message
357  } else {
358  // Successfully got message
360  }
362  _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
363  memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
364  break;
365  }
366 
367  bufferIndex++;
368  // If a message has been sucessfully decoded, check index
369  if (status->msg_received == MAVLINK_FRAMING_OK)
370  {
371  //while(status->current_seq != rxmsg->seq)
372  //{
373  // status->packet_rx_drop_count++;
374  // status->current_seq++;
375  //}
376  status->current_rx_seq = rxmsg->seq;
377  // Initial condition: If no packet has been received so far, drop count is undefined
378  if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
379  // Count this packet as received
380  status->packet_rx_success_count++;
381  }
382 
383  r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
384  r_mavlink_status->parse_state = status->parse_state;
385  r_mavlink_status->packet_idx = status->packet_idx;
386  r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
387  r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
388  r_mavlink_status->packet_rx_drop_count = status->parse_error;
389  status->parse_error = 0;
390 
391  if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
392  /*
393  the CRC came out wrong. We now need to overwrite the
394  msg CRC with the one on the wire so that if the
395  caller decides to forward the message anyway that
396  mavlink_msg_to_send_buffer() won't overwrite the
397  checksum
398  */
399  r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
400  }
401 
402  return status->msg_received;
403 }
404 
447 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
448 {
451  c,
452  r_message,
453  r_mavlink_status);
454 }
455 
456 
498 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
499 {
500  uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
501  if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
502  // we got a bad CRC. Treat as a parse failure
505  status->parse_error++;
508  if (c == MAVLINK_STX)
509  {
511  rxmsg->len = 0;
512  mavlink_start_checksum(rxmsg);
513  }
514  return 0;
515  }
516  return msg_received;
517 }
518 
529 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)
530 {
531  uint16_t bits_remain = bits;
532  // Transform number into network order
533  int32_t v;
534  uint8_t i_bit_index, i_byte_index, curr_bits_n;
535 #if MAVLINK_NEED_BYTE_SWAP
536  union {
537  int32_t i;
538  uint8_t b[4];
539  } bin, bout;
540  bin.i = b;
541  bout.b[0] = bin.b[3];
542  bout.b[1] = bin.b[2];
543  bout.b[2] = bin.b[1];
544  bout.b[3] = bin.b[0];
545  v = bout.i;
546 #else
547  v = b;
548 #endif
549 
550  // buffer in
551  // 01100000 01000000 00000000 11110001
552  // buffer out
553  // 11110001 00000000 01000000 01100000
554 
555  // Existing partly filled byte (four free slots)
556  // 0111xxxx
557 
558  // Mask n free bits
559  // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
560  // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
561 
562  // Shift n bits into the right position
563  // out = in >> n;
564 
565  // Mask and shift bytes
566  i_bit_index = bit_index;
567  i_byte_index = packet_index;
568  if (bit_index > 0)
569  {
570  // If bits were available at start, they were available
571  // in the byte before the current index
572  i_byte_index--;
573  }
574 
575  // While bits have not been packed yet
576  while (bits_remain > 0)
577  {
578  // Bits still have to be packed
579  // there can be more than 8 bits, so
580  // we might have to pack them into more than one byte
581 
582  // First pack everything we can into the current 'open' byte
583  //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
584  //FIXME
585  if (bits_remain <= (uint8_t)(8 - i_bit_index))
586  {
587  // Enough space
588  curr_bits_n = (uint8_t)bits_remain;
589  }
590  else
591  {
592  curr_bits_n = (8 - i_bit_index);
593  }
594 
595  // Pack these n bits into the current byte
596  // Mask out whatever was at that position with ones (xxx11111)
597  buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
598  // Put content to this position, by masking out the non-used part
599  buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
600 
601  // Increment the bit index
602  i_bit_index += curr_bits_n;
603 
604  // Now proceed to the next byte, if necessary
605  bits_remain -= curr_bits_n;
606  if (bits_remain > 0)
607  {
608  // Offer another 8 bits / one byte
609  i_byte_index++;
610  i_bit_index = 0;
611  }
612  }
613 
614  *r_bit_index = i_bit_index;
615  // If a partly filled byte is present, mark this as consumed
616  if (i_bit_index != 7) i_byte_index++;
617  return i_byte_index - packet_index;
618 }
619 
620 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
621 
622 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
623 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
624 // whole packet at a time
625 
626 /*
627 
628 #include "mavlink_types.h"
629 
630 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
631 {
632  if (chan == MAVLINK_COMM_0)
633  {
634  uart0_transmit(ch);
635  }
636  if (chan == MAVLINK_COMM_1)
637  {
638  uart1_transmit(ch);
639  }
640 }
641  */
642 
643 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
644 {
645 #ifdef MAVLINK_SEND_UART_BYTES
646  /* this is the more efficient approach, if the platform
647  defines it */
648  MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
649 #else
650  /* fallback to one byte at a time */
651  uint16_t i;
652  for (i = 0; i < len; i++) {
653  comm_send_ch(chan, (uint8_t)buf[i]);
654  }
655 #endif
656 }
657 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
658 
659 #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