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         int bufferIndex = 0;
 293
 294         status->msg_received = 0;
 295
 296         switch (status->parse_state)
 297         {
 298         case MAVLINK_PARSE_STATE_UNINIT:
 299         case MAVLINK_PARSE_STATE_IDLE:
 300                 if (c == MAVLINK_STX)
 301                 {
 302                         status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 303                         rxmsg->len = 0;
 304                         rxmsg->magic = c;
 305                         mavlink_start_checksum(rxmsg);
 306                 }
 307                 break;
 308
 309         case MAVLINK_PARSE_STATE_GOT_STX:
 310                         if (status->msg_received
 311 /* Support shorter buffers than the
 312    default maximum packet size */
 313 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
 314                                 || c > MAVLINK_MAX_PAYLOAD_LEN
 315 #endif
 316                                 )
 317                 {
 318                         status->buffer_overrun++;
 319                         status->parse_error++;
 320                         status->msg_received = 0;
 321                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 322                 }
 323                 else
 324                 {
 325                         // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
 326                         rxmsg->len = c;
 327                         status->packet_idx = 0;
 328                         mavlink_update_checksum(rxmsg, c);
 329                         status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
 330                 }
 331                 break;
 332
 333         case MAVLINK_PARSE_STATE_GOT_LENGTH:
 334                 rxmsg->seq = c;
 335                 mavlink_update_checksum(rxmsg, c);
 336                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
 337                 break;
 338
 339         case MAVLINK_PARSE_STATE_GOT_SEQ:
 340                 rxmsg->sysid = c;
 341                 mavlink_update_checksum(rxmsg, c);
 342                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
 343                 break;
 344
 345         case MAVLINK_PARSE_STATE_GOT_SYSID:
 346                 rxmsg->compid = c;
 347                 mavlink_update_checksum(rxmsg, c);
 348                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
 349                 break;
 350
 351         case MAVLINK_PARSE_STATE_GOT_COMPID:
 352 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
 353                 if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
 354                 {
 355                         status->parse_error++;
 356                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 357                         break;
 358             }
 359 #endif
 360                 rxmsg->msgid = c;
 361                 mavlink_update_checksum(rxmsg, c);
 362                 if (rxmsg->len == 0)
 363                 {
 364                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
 365                 }
 366                 else
 367                 {
 368                         status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
 369                 }
 370                 break;
 371
 372         case MAVLINK_PARSE_STATE_GOT_MSGID:
 373                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
 374                 mavlink_update_checksum(rxmsg, c);
 375                 if (status->packet_idx == rxmsg->len)
 376                 {
 377                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
 378                 }
 379                 break;
 380
 381         case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
 382 #if MAVLINK_CRC_EXTRA
 383                 mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
 384 #endif
 385                 if (c != (rxmsg->checksum & 0xFF)) {
 386                         // Check first checksum byte
 387                         status->parse_error++;
 388                         status->msg_received = 0;
 389                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 390                         if (c == MAVLINK_STX)
 391                         {
 392                                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 393                                 rxmsg->len = 0;
 394                                 mavlink_start_checksum(rxmsg);
 395                         }
 396                 }
 397                 else
 398                 {
 399                         status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
 400                         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
 401                 }
 402                 break;
 403
 404         case MAVLINK_PARSE_STATE_GOT_CRC1:
 405                 if (c != (rxmsg->checksum >> 8)) {
 406                         // Check second checksum byte
 407                         status->parse_error++;
 408                         status->msg_received = 0;
 409                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 410                         if (c == MAVLINK_STX)
 411                         {
 412                                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 413                                 rxmsg->len = 0;
 414                                 mavlink_start_checksum(rxmsg);
 415                         }
 416                 }
 417                 else
 418                 {
 419                         // Successfully got message
 420                         status->msg_received = 1;
 421                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 422                         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
 423                         memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
 424                 }
 425                 break;
 426         }
 427
 428         bufferIndex++;
 429         // If a message has been sucessfully decoded, check index
 430         if (status->msg_received == 1)
 431         {
 432                 //while(status->current_seq != rxmsg->seq)
 433                 //{
 434                 //      status->packet_rx_drop_count++;
 435                 //               status->current_seq++;
 436                 //}
 437                 status->current_rx_seq = rxmsg->seq;
 438                 // Initial condition: If no packet has been received so far, drop count is undefined
 439                 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
 440                 // Count this packet as received
 441                 status->packet_rx_success_count++;
 442         }
 443
 444         r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
 445         r_mavlink_status->parse_state = status->parse_state;
 446         r_mavlink_status->packet_idx = status->packet_idx;
 447         r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
 448         r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
 449         r_mavlink_status->packet_rx_drop_count = status->parse_error;
 450         status->parse_error = 0;
 451         return status->msg_received;
 452 }
 453
 454 /**
 455  * @brief Put a bitfield of length 1-32 bit into the buffer
 456  *
 457  * @param b the value to add, will be encoded in the bitfield
 458  * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
 459  * @param packet_index the position in the packet (the index of the first byte to use)
 460  * @param bit_index the position in the byte (the index of the first bit to use)
 461  * @param buffer packet buffer to write into
 462  * @return new position of the last used byte in the buffer
 463  */
 464 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)
 465 {
 466         uint16_t bits_remain = bits;
 467         // Transform number into network order
 468         int32_t v;
 469         uint8_t i_bit_index, i_byte_index, curr_bits_n;
 470 #if MAVLINK_NEED_BYTE_SWAP
 471         union {
 472                 int32_t i;
 473                 uint8_t b[4];
 474         } bin, bout;
 475         bin.i = b;
 476         bout.b[0] = bin.b[3];
 477         bout.b[1] = bin.b[2];
 478         bout.b[2] = bin.b[1];
 479         bout.b[3] = bin.b[0];
 480         v = bout.i;
 481 #else
 482         v = b;
 483 #endif
 484
 485         // buffer in
 486         // 01100000 01000000 00000000 11110001
 487         // buffer out
 488         // 11110001 00000000 01000000 01100000
 489
 490         // Existing partly filled byte (four free slots)
 491         // 0111xxxx
 492
 493         // Mask n free bits
 494         // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
 495         // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
 496
 497         // Shift n bits into the right position
 498         // out = in >> n;
 499
 500         // Mask and shift bytes
 501         i_bit_index = bit_index;
 502         i_byte_index = packet_index;
 503         if (bit_index > 0)
 504         {
 505                 // If bits were available at start, they were available
 506                 // in the byte before the current index
 507                 i_byte_index--;
 508         }
 509
 510         // While bits have not been packed yet
 511         while (bits_remain > 0)
 512         {
 513                 // Bits still have to be packed
 514                 // there can be more than 8 bits, so
 515                 // we might have to pack them into more than one byte
 516
 517                 // First pack everything we can into the current 'open' byte
 518                 //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
 519                 //FIXME
 520                 if (bits_remain <= (uint8_t)(8 - i_bit_index))
 521                 {
 522                         // Enough space
 523                         curr_bits_n = (uint8_t)bits_remain;
 524                 }
 525                 else
 526                 {
 527                         curr_bits_n = (8 - i_bit_index);
 528                 }
 529
 530                 // Pack these n bits into the current byte
 531                 // Mask out whatever was at that position with ones (xxx11111)
 532                 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
 533                 // Put content to this position, by masking out the non-used part
 534                 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
 535
 536                 // Increment the bit index
 537                 i_bit_index += curr_bits_n;
 538
 539                 // Now proceed to the next byte, if necessary
 540                 bits_remain -= curr_bits_n;
 541                 if (bits_remain > 0)
 542                 {
 543                         // Offer another 8 bits / one byte
 544                         i_byte_index++;
 545                         i_bit_index = 0;
 546                 }
 547         }
 548
 549         *r_bit_index = i_bit_index;
 550         // If a partly filled byte is present, mark this as consumed
 551         if (i_bit_index != 7) i_byte_index++;
 552         return i_byte_index - packet_index;
 553 }
 554
 555 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
 556
 557 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
 558 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
 559 // whole packet at a time
 560
 561 /*
 562
 563 #include "mavlink_types.h"
 564
 565 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
 566 {
 567     if (chan == MAVLINK_COMM_0)
 568     {
 569         uart0_transmit(ch);
 570     }
 571     if (chan == MAVLINK_COMM_1)
 572     {
 573         uart1_transmit(ch);
 574     }
 575 }
 576  */
 577
 578 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
 579 {
 580 #ifdef MAVLINK_SEND_UART_BYTES
 581         /* this is the more efficient approach, if the platform
 582            defines it */
 583         MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
 584 #else
 585         /* fallback to one byte at a time */
 586         uint16_t i;
 587         for (i = 0; i < len; i++) {
 588                 comm_send_ch(chan, (uint8_t)buf[i]);
 589         }
 590 #endif
 591 }
 592 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
 593
 594 #endif /* _MAVLINK_HELPERS_H_ */