lib/main/MAVLink/mavlink_helpers.h

   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
  17 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
  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
  46 /**
  47  * @brief Reset the status of a channel.
  48  */
  49 MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
  50 {
  51         mavlink_status_t *status = mavlink_get_channel_status(chan);
  52         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  53 }
  54
  55 /**
  56  * @brief Finalize a MAVLink message with channel assignment
  57  *
  58  * This function calculates the checksum and sets length and aircraft id correctly.
  59  * It assumes that the message id and the payload are already correctly set. This function
  60  * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
  61  * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
  62  *
  63  * @param msg Message to finalize
  64  * @param system_id Id of the sending (this) system, 1-127
  65  * @param length Message length
  66  */
  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
  81         msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
  82         mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
  83         msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
  84 // msg->checksum is actually aligned
  85 #pragma GCC diagnostic push
  86 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
  87         crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
  88 #pragma GCC diagnostic pop
  89
  90 #if MAVLINK_CRC_EXTRA
  91 // msg->checksum is actually aligned
  92 #pragma GCC diagnostic push
  93 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
  94         crc_accumulate(crc_extra, &msg->checksum);
  95 #pragma GCC diagnostic pop
  96 #endif
  97         mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
  98         mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
  99
 100         return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 101 }
 102
 103
 104 /**
 105  * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
 106  */
 107 #if MAVLINK_CRC_EXTRA
 108 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
 109                                                  uint8_t length, uint8_t crc_extra)
 110 {
 111         return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
 112 }
 113 #else
 114 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
 115                                                  uint8_t length)
 116 {
 117         return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
 118 }
 119 #endif
 120
 121 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
 122 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
 123
 124 /**
 125  * @brief Finalize a MAVLink message with channel assignment and send
 126  */
 127 #if MAVLINK_CRC_EXTRA
 128 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
 129                                                     uint8_t length, uint8_t crc_extra)
 130 #else
 131 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
 132 #endif
 133 {
 134         uint16_t checksum;
 135         uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
 136         uint8_t ck[2];
 137         mavlink_status_t *status = mavlink_get_channel_status(chan);
 138         buf[0] = MAVLINK_STX;
 139         buf[1] = length;
 140         buf[2] = status->current_tx_seq;
 141         buf[3] = mavlink_system.sysid;
 142         buf[4] = mavlink_system.compid;
 143         buf[5] = msgid;
 144         status->current_tx_seq++;
 145         checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
 146         crc_accumulate_buffer(&checksum, packet, length);
 147 #if MAVLINK_CRC_EXTRA
 148         crc_accumulate(crc_extra, &checksum);
 149 #endif
 150         ck[0] = (uint8_t)(checksum & 0xFF);
 151         ck[1] = (uint8_t)(checksum >> 8);
 152
 153         MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
 154         _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
 155         _mavlink_send_uart(chan, packet, length);
 156         _mavlink_send_uart(chan, (const char *)ck, 2);
 157         MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
 158 }
 159
 160 /**
 161  * @brief re-send a message over a uart channel
 162  * this is more stack efficient than re-marshalling the message
 163  */
 164 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
 165 {
 166         uint8_t ck[2];
 167
 168         ck[0] = (uint8_t)(msg->checksum & 0xFF);
 169         ck[1] = (uint8_t)(msg->checksum >> 8);
 170         // XXX use the right sequence here
 171
 172         MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
 173         _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
 174         _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
 175         _mavlink_send_uart(chan, (const char *)ck, 2);
 176         MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
 177 }
 178 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
 179
 180 /**
 181  * @brief Pack a message to send it over a serial byte stream
 182  */
 183 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
 184 {
 185         memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
 186
 187         uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
 188
 189         ck[0] = (uint8_t)(msg->checksum & 0xFF);
 190         ck[1] = (uint8_t)(msg->checksum >> 8);
 191
 192         return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
 193 }
 194
 195 union __mavlink_bitfield {
 196         uint8_t uint8;
 197         int8_t int8;
 198         uint16_t uint16;
 199         int16_t int16;
 200         uint32_t uint32;
 201         int32_t int32;
 202 };
 203
 204
 205 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
 206 {
 207 // msg->checksum is actually aligned
 208 #pragma GCC diagnostic push
 209 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
 210         crc_init(&msg->checksum);
 211 #pragma GCC diagnostic pop
 212 }
 213
 214 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
 215 {
 216 // msg->checksum is actually aligned
 217 #pragma GCC diagnostic push
 218 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
 219         crc_accumulate(c, &msg->checksum);
 220 #pragma GCC diagnostic pop
 221 }
 222
 223 /**
 224  * This is a convenience function which handles the complete MAVLink parsing.
 225  * the function will parse one byte at a time and return the complete packet once
 226  * it could be successfully decoded. Checksum and other failures will be silently
 227  * ignored.
 228  *
 229  * Messages are parsed into an internal buffer (one for each channel). When a complete
 230  * message is received it is copies into *returnMsg and the channel's status is
 231  * copied into *returnStats.
 232  *
 233  * @param chan     ID of the current channel. This allows to parse different channels with this function.
 234  *                 a channel is not a physical message channel like a serial port, but a logic partition of
 235  *                 the communication streams in this case. COMM_NB is the limit for the number of channels
 236  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
 237  * @param c        The char to parse
 238  *
 239  * @param returnMsg NULL if no message could be decoded, the message data else
 240  * @param returnStats if a message was decoded, this is filled with the channel's stats
 241  * @return 0 if no message could be decoded, 1 else
 242  *
 243  * A typical use scenario of this function call is:
 244  *
 245  * @code
 246  * #include <mavlink.h>
 247  *
 248  * mavlink_message_t msg;
 249  * int chan = 0;
 250  *
 251  *
 252  * while(serial.bytesAvailable > 0)
 253  * {
 254  *   uint8_t byte = serial.getNextByte();
 255  *   if (mavlink_parse_char(chan, byte, &msg))
 256  *     {
 257  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
 258  *     }
 259  * }
 260  *
 261  *
 262  * @endcode
 263  */
 264 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
 265 {
 266         /*
 267           default message crc function. You can override this per-system to
 268           put this data in a different memory segment
 269         */
 270 #if MAVLINK_CRC_EXTRA
 271 #ifndef MAVLINK_MESSAGE_CRC
 272         static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 273 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
 274 #endif
 275 #endif
 276
 277 /* Enable this option to check the length of each message.
 278  This allows invalid messages to be caught much sooner. Use if the transmission
 279  medium is prone to missing (or extra) characters (e.g. a radio that fades in
 280  and out). Only use if the channel will only contain messages types listed in
 281  the headers.
 282 */
 283 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
 284 #ifndef MAVLINK_MESSAGE_LENGTH
 285         static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 286 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
 287 #endif
 288 #endif
 289
 290         mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message
 291         mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
 292
 293         status->msg_received = 0;
 294
 295         switch (status->parse_state)
 296         {
 297         case MAVLINK_PARSE_STATE_UNINIT:
 298         case MAVLINK_PARSE_STATE_IDLE:
 299                 if (c == MAVLINK_STX)
 300                 {
 301                         status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 302                         rxmsg->len = 0;
 303                         rxmsg->magic = c;
 304                         mavlink_start_checksum(rxmsg);
 305                 }
 306                 break;
 307
 308         case MAVLINK_PARSE_STATE_GOT_STX:
 309                         if (status->msg_received
 310 /* Support shorter buffers than the
 311    default maximum packet size */
 312 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
 313                                 || c > MAVLINK_MAX_PAYLOAD_LEN
 314 #endif
 315                                 )
 316                 {
 317                         status->buffer_overrun++;
 318                         status->parse_error++;
 319                         status->msg_received = 0;
 320                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 321                 }
 322                 else
 323                 {
 324                         // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
 325                         rxmsg->len = c;
 326                         status->packet_idx = 0;
 327                         mavlink_update_checksum(rxmsg, c);
 328                         status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
 329                 }
 330                 break;
 331
 332         case MAVLINK_PARSE_STATE_GOT_LENGTH:
 333                 rxmsg->seq = c;
 334                 mavlink_update_checksum(rxmsg, c);
 335                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
 336                 break;
 337
 338         case MAVLINK_PARSE_STATE_GOT_SEQ:
 339                 rxmsg->sysid = c;
 340                 mavlink_update_checksum(rxmsg, c);
 341                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
 342                 break;
 343
 344         case MAVLINK_PARSE_STATE_GOT_SYSID:
 345                 rxmsg->compid = c;
 346                 mavlink_update_checksum(rxmsg, c);
 347                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
 348                 break;
 349
 350         case MAVLINK_PARSE_STATE_GOT_COMPID:
 351 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
 352                 if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
 353                 {
 354                         status->parse_error++;
 355                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 356                         break;
 357             }
 358 #endif
 359                 rxmsg->msgid = c;
 360                 mavlink_update_checksum(rxmsg, c);
 361                 if (rxmsg->len == 0)
 362                 {
 363                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
 364                 }
 365                 else
 366                 {
 367                         status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
 368                 }
 369                 break;
 370
 371         case MAVLINK_PARSE_STATE_GOT_MSGID:
 372                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
 373                 mavlink_update_checksum(rxmsg, c);
 374                 if (status->packet_idx == rxmsg->len)
 375                 {
 376                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
 377                 }
 378                 break;
 379
 380         case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
 381 #if MAVLINK_CRC_EXTRA
 382                 mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
 383 #endif
 384                 if (c != (rxmsg->checksum & 0xFF)) {
 385                         // Check first checksum byte
 386                         status->parse_error++;
 387                         status->msg_received = 0;
 388                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 389                         if (c == MAVLINK_STX)
 390                         {
 391                                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 392                                 rxmsg->len = 0;
 393                                 mavlink_start_checksum(rxmsg);
 394                         }
 395                 }
 396                 else
 397                 {
 398                         status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
 399                         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
 400                 }
 401                 break;
 402
 403         case MAVLINK_PARSE_STATE_GOT_CRC1:
 404                 if (c != (rxmsg->checksum >> 8)) {
 405                         // Check second checksum byte
 406                         status->parse_error++;
 407                         status->msg_received = 0;
 408                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 409                         if (c == MAVLINK_STX)
 410                         {
 411                                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 412                                 rxmsg->len = 0;
 413                                 mavlink_start_checksum(rxmsg);
 414                         }
 415                 }
 416                 else
 417                 {
 418                         // Successfully got message
 419                         status->msg_received = 1;
 420                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 421                         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
 422                         memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
 423                 }
 424                 break;
 425         }
 426
 427         // If a message has been sucessfully decoded, check index
 428         if (status->msg_received == 1)
 429         {
 430                 //while(status->current_seq != rxmsg->seq)
 431                 //{
 432                 //      status->packet_rx_drop_count++;
 433                 //               status->current_seq++;
 434                 //}
 435                 status->current_rx_seq = rxmsg->seq;
 436                 // Initial condition: If no packet has been received so far, drop count is undefined
 437                 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
 438                 // Count this packet as received
 439                 status->packet_rx_success_count++;
 440         }
 441
 442         r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
 443         r_mavlink_status->parse_state = status->parse_state;
 444         r_mavlink_status->packet_idx = status->packet_idx;
 445         r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
 446         r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
 447         r_mavlink_status->packet_rx_drop_count = status->parse_error;
 448         status->parse_error = 0;
 449         return status->msg_received;
 450 }
 451
 452 /**
 453  * @brief Put a bitfield of length 1-32 bit into the buffer
 454  *
 455  * @param b the value to add, will be encoded in the bitfield
 456  * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
 457  * @param packet_index the position in the packet (the index of the first byte to use)
 458  * @param bit_index the position in the byte (the index of the first bit to use)
 459  * @param buffer packet buffer to write into
 460  * @return new position of the last used byte in the buffer
 461  */
 462 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)
 463 {
 464         uint16_t bits_remain = bits;
 465         // Transform number into network order
 466         int32_t v;
 467         uint8_t i_bit_index, i_byte_index, curr_bits_n;
 468 #if MAVLINK_NEED_BYTE_SWAP
 469         union {
 470                 int32_t i;
 471                 uint8_t b[4];
 472         } bin, bout;
 473         bin.i = b;
 474         bout.b[0] = bin.b[3];
 475         bout.b[1] = bin.b[2];
 476         bout.b[2] = bin.b[1];
 477         bout.b[3] = bin.b[0];
 478         v = bout.i;
 479 #else
 480         v = b;
 481 #endif
 482
 483         // buffer in
 484         // 01100000 01000000 00000000 11110001
 485         // buffer out
 486         // 11110001 00000000 01000000 01100000
 487
 488         // Existing partly filled byte (four free slots)
 489         // 0111xxxx
 490
 491         // Mask n free bits
 492         // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
 493         // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
 494
 495         // Shift n bits into the right position
 496         // out = in >> n;
 497
 498         // Mask and shift bytes
 499         i_bit_index = bit_index;
 500         i_byte_index = packet_index;
 501         if (bit_index > 0)
 502         {
 503                 // If bits were available at start, they were available
 504                 // in the byte before the current index
 505                 i_byte_index--;
 506         }
 507
 508         // While bits have not been packed yet
 509         while (bits_remain > 0)
 510         {
 511                 // Bits still have to be packed
 512                 // there can be more than 8 bits, so
 513                 // we might have to pack them into more than one byte
 514
 515                 // First pack everything we can into the current 'open' byte
 516                 //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
 517                 //FIXME
 518                 if (bits_remain <= (uint8_t)(8 - i_bit_index))
 519                 {
 520                         // Enough space
 521                         curr_bits_n = (uint8_t)bits_remain;
 522                 }
 523                 else
 524                 {
 525                         curr_bits_n = (8 - i_bit_index);
 526                 }
 527
 528                 // Pack these n bits into the current byte
 529                 // Mask out whatever was at that position with ones (xxx11111)
 530                 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
 531                 // Put content to this position, by masking out the non-used part
 532                 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
 533
 534                 // Increment the bit index
 535                 i_bit_index += curr_bits_n;
 536
 537                 // Now proceed to the next byte, if necessary
 538                 bits_remain -= curr_bits_n;
 539                 if (bits_remain > 0)
 540                 {
 541                         // Offer another 8 bits / one byte
 542                         i_byte_index++;
 543                         i_bit_index = 0;
 544                 }
 545         }
 546
 547         *r_bit_index = i_bit_index;
 548         // If a partly filled byte is present, mark this as consumed
 549         if (i_bit_index != 7) i_byte_index++;
 550         return i_byte_index - packet_index;
 551 }
 552
 553 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
 554
 555 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
 556 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
 557 // whole packet at a time
 558
 559 /*
 560
 561 #include "mavlink_types.h"
 562
 563 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
 564 {
 565     if (chan == MAVLINK_COMM_0)
 566     {
 567         uart0_transmit(ch);
 568     }
 569     if (chan == MAVLINK_COMM_1)
 570     {
 571         uart1_transmit(ch);
 572     }
 573 }
 574  */
 575
 576 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
 577 {
 578 #ifdef MAVLINK_SEND_UART_BYTES
 579         /* this is the more efficient approach, if the platform
 580            defines it */
 581         MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
 582 #else
 583         /* fallback to one byte at a time */
 584         uint16_t i;
 585         for (i = 0; i < len; i++) {
 586                 comm_send_ch(chan, (uint8_t)buf[i]);
 587         }
 588 #endif
 589 }
 590 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
 591
 592 #endif /* _MAVLINK_HELPERS_H_ */