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