mavlink_helpers.h
Go to the documentation of this file.
00001 #ifndef  _MAVLINK_HELPERS_H_
00002 #define  _MAVLINK_HELPERS_H_
00003 
00004 #include "string.h"
00005 #include "checksum.h"
00006 #include "mavlink_types.h"
00007 #include "mavlink_conversions.h"
00008 
00009 #ifndef MAVLINK_HELPER
00010 #define MAVLINK_HELPER
00011 #endif
00012 
00013 /*
00014  * Internal function to give access to the channel status for each channel
00015  */
00016 #ifndef MAVLINK_GET_CHANNEL_STATUS
00017 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
00018 {
00019 #ifdef MAVLINK_EXTERNAL_RX_STATUS
00020         // No m_mavlink_status array defined in function,
00021         // has to be defined externally
00022 #else
00023         static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
00024 #endif
00025         return &m_mavlink_status[chan];
00026 }
00027 #endif
00028 
00029 /*
00030  * Internal function to give access to the channel buffer for each channel
00031  */
00032 #ifndef MAVLINK_GET_CHANNEL_BUFFER
00033 MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
00034 {
00035         
00036 #ifdef MAVLINK_EXTERNAL_RX_BUFFER
00037         // No m_mavlink_buffer array defined in function,
00038         // has to be defined externally
00039 #else
00040         static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
00041 #endif
00042         return &m_mavlink_buffer[chan];
00043 }
00044 #endif
00045 
00049 MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
00050 {
00051         mavlink_status_t *status = mavlink_get_channel_status(chan);
00052         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00053 }
00054 
00067 #if MAVLINK_CRC_EXTRA
00068 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00069                                                       uint8_t chan, uint8_t length, uint8_t crc_extra)
00070 #else
00071 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00072                                                       uint8_t chan, uint8_t length)
00073 #endif
00074 {
00075         // This code part is the same for all messages;
00076         msg->magic = MAVLINK_STX;
00077         msg->len = length;
00078         msg->sysid = system_id;
00079         msg->compid = component_id;
00080         // One sequence number per channel
00081         msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
00082         mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
00083         msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
00084         crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
00085 #if MAVLINK_CRC_EXTRA
00086         crc_accumulate(crc_extra, &msg->checksum);
00087 #endif
00088         mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
00089         mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
00090 
00091         return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00092 }
00093 
00094 
00098 #if MAVLINK_CRC_EXTRA
00099 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00100                                                  uint8_t length, uint8_t crc_extra)
00101 {
00102         return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
00103 }
00104 #else
00105 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00106                                                  uint8_t length)
00107 {
00108         return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
00109 }
00110 #endif
00111 
00112 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00113 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
00114 
00118 #if MAVLINK_CRC_EXTRA
00119 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
00120                                                     uint8_t length, uint8_t crc_extra)
00121 #else
00122 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
00123 #endif
00124 {
00125         uint16_t checksum;
00126         uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
00127         uint8_t ck[2];
00128         mavlink_status_t *status = mavlink_get_channel_status(chan);
00129         buf[0] = MAVLINK_STX;
00130         buf[1] = length;
00131         buf[2] = status->current_tx_seq;
00132         buf[3] = mavlink_system.sysid;
00133         buf[4] = mavlink_system.compid;
00134         buf[5] = msgid;
00135         status->current_tx_seq++;
00136         checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
00137         crc_accumulate_buffer(&checksum, packet, length);
00138 #if MAVLINK_CRC_EXTRA
00139         crc_accumulate(crc_extra, &checksum);
00140 #endif
00141         ck[0] = (uint8_t)(checksum & 0xFF);
00142         ck[1] = (uint8_t)(checksum >> 8);
00143 
00144         MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
00145         _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
00146         _mavlink_send_uart(chan, packet, length);
00147         _mavlink_send_uart(chan, (const char *)ck, 2);
00148         MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
00149 }
00150 
00155 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
00156 {
00157         uint8_t ck[2];
00158 
00159         ck[0] = (uint8_t)(msg->checksum & 0xFF);
00160         ck[1] = (uint8_t)(msg->checksum >> 8);
00161         // XXX use the right sequence here
00162 
00163         MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
00164         _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
00165         _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
00166         _mavlink_send_uart(chan, (const char *)ck, 2);
00167         MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
00168 }
00169 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
00170 
00174 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
00175 {
00176         memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
00177 
00178         uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
00179 
00180         ck[0] = (uint8_t)(msg->checksum & 0xFF);
00181         ck[1] = (uint8_t)(msg->checksum >> 8);
00182 
00183         return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
00184 }
00185 
00186 union __mavlink_bitfield {
00187         uint8_t uint8;
00188         int8_t int8;
00189         uint16_t uint16;
00190         int16_t int16;
00191         uint32_t uint32;
00192         int32_t int32;
00193 };
00194 
00195 
00196 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
00197 {
00198         crc_init(&msg->checksum);
00199 }
00200 
00201 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
00202 {
00203         crc_accumulate(c, &msg->checksum);
00204 }
00205 
00240 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
00241                                                  mavlink_status_t* status,
00242                                                  uint8_t c, 
00243                                                  mavlink_message_t* r_message, 
00244                                                  mavlink_status_t* r_mavlink_status)
00245 {
00246         /*
00247           default message crc function. You can override this per-system to
00248           put this data in a different memory segment
00249         */
00250 #if MAVLINK_CRC_EXTRA
00251 #ifndef MAVLINK_MESSAGE_CRC
00252         static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
00253 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
00254 #endif
00255 #endif
00256 
00257         /* Enable this option to check the length of each message.
00258            This allows invalid messages to be caught much sooner. Use if the transmission
00259            medium is prone to missing (or extra) characters (e.g. a radio that fades in
00260            and out). Only use if the channel will only contain messages types listed in
00261            the headers.
00262         */
00263 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00264 #ifndef MAVLINK_MESSAGE_LENGTH
00265         static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
00266 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
00267 #endif
00268 #endif
00269 
00270         int bufferIndex = 0;
00271 
00272         status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00273 
00274         switch (status->parse_state)
00275         {
00276         case MAVLINK_PARSE_STATE_UNINIT:
00277         case MAVLINK_PARSE_STATE_IDLE:
00278                 if (c == MAVLINK_STX)
00279                 {
00280                         status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00281                         rxmsg->len = 0;
00282                         rxmsg->magic = c;
00283                         mavlink_start_checksum(rxmsg);
00284                 }
00285                 break;
00286 
00287         case MAVLINK_PARSE_STATE_GOT_STX:
00288                         if (status->msg_received 
00289 /* Support shorter buffers than the
00290    default maximum packet size */
00291 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
00292                                 || c > MAVLINK_MAX_PAYLOAD_LEN
00293 #endif
00294                                 )
00295                 {
00296                         status->buffer_overrun++;
00297                         status->parse_error++;
00298                         status->msg_received = 0;
00299                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00300                 }
00301                 else
00302                 {
00303                         // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00304                         rxmsg->len = c;
00305                         status->packet_idx = 0;
00306                         mavlink_update_checksum(rxmsg, c);
00307                         status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00308                 }
00309                 break;
00310 
00311         case MAVLINK_PARSE_STATE_GOT_LENGTH:
00312                 rxmsg->seq = c;
00313                 mavlink_update_checksum(rxmsg, c);
00314                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00315                 break;
00316 
00317         case MAVLINK_PARSE_STATE_GOT_SEQ:
00318                 rxmsg->sysid = c;
00319                 mavlink_update_checksum(rxmsg, c);
00320                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00321                 break;
00322 
00323         case MAVLINK_PARSE_STATE_GOT_SYSID:
00324                 rxmsg->compid = c;
00325                 mavlink_update_checksum(rxmsg, c);
00326                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00327                 break;
00328 
00329         case MAVLINK_PARSE_STATE_GOT_COMPID:
00330 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00331                 if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
00332                 {
00333                         status->parse_error++;
00334                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00335                         break;
00336             }
00337 #endif
00338                 rxmsg->msgid = c;
00339                 mavlink_update_checksum(rxmsg, c);
00340                 if (rxmsg->len == 0)
00341                 {
00342                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00343                 }
00344                 else
00345                 {
00346                         status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
00347                 }
00348                 break;
00349 
00350         case MAVLINK_PARSE_STATE_GOT_MSGID:
00351                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
00352                 mavlink_update_checksum(rxmsg, c);
00353                 if (status->packet_idx == rxmsg->len)
00354                 {
00355                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00356                 }
00357                 break;
00358 
00359         case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00360 #if MAVLINK_CRC_EXTRA
00361                 mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
00362 #endif
00363                 if (c != (rxmsg->checksum & 0xFF)) {
00364                         status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
00365                 } else {
00366                         status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00367                 }
00368                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
00369                 break;
00370 
00371         case MAVLINK_PARSE_STATE_GOT_CRC1:
00372         case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
00373                 if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
00374                         // got a bad CRC message
00375                         status->msg_received = MAVLINK_FRAMING_BAD_CRC;
00376                 } else {
00377                         // Successfully got message
00378                         status->msg_received = MAVLINK_FRAMING_OK;
00379                 }
00380                 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00381                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
00382                 memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00383                 break;
00384         }
00385 
00386         bufferIndex++;
00387         // If a message has been sucessfully decoded, check index
00388         if (status->msg_received == MAVLINK_FRAMING_OK)
00389         {
00390                 //while(status->current_seq != rxmsg->seq)
00391                 //{
00392                 //      status->packet_rx_drop_count++;
00393                 //               status->current_seq++;
00394                 //}
00395                 status->current_rx_seq = rxmsg->seq;
00396                 // Initial condition: If no packet has been received so far, drop count is undefined
00397                 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00398                 // Count this packet as received
00399                 status->packet_rx_success_count++;
00400         }
00401 
00402         r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
00403         r_mavlink_status->parse_state = status->parse_state;
00404         r_mavlink_status->packet_idx = status->packet_idx;
00405         r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
00406         r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
00407         r_mavlink_status->packet_rx_drop_count = status->parse_error;
00408         status->parse_error = 0;
00409 
00410         if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
00411                 /*
00412                   the CRC came out wrong. We now need to overwrite the
00413                   msg CRC with the one on the wire so that if the
00414                   caller decides to forward the message anyway that
00415                   mavlink_msg_to_send_buffer() won't overwrite the
00416                   checksum
00417                  */
00418                 r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
00419         }
00420 
00421         return status->msg_received;
00422 }
00423 
00465 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00466 {
00467         return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
00468                                          mavlink_get_channel_status(chan),
00469                                          c,
00470                                          r_message,
00471                                          r_mavlink_status);
00472 }
00473 
00474 
00515 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00516 {
00517     uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
00518     if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
00519             // we got a bad CRC. Treat as a parse failure
00520             mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
00521             mavlink_status_t* status = mavlink_get_channel_status(chan);
00522             status->parse_error++;
00523             status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00524             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00525             if (c == MAVLINK_STX)
00526             {
00527                     status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00528                     rxmsg->len = 0;
00529                     mavlink_start_checksum(rxmsg);
00530             }
00531             return 0;
00532     }
00533     return msg_received;
00534 }
00535 
00546 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)
00547 {
00548         uint16_t bits_remain = bits;
00549         // Transform number into network order
00550         int32_t v;
00551         uint8_t i_bit_index, i_byte_index, curr_bits_n;
00552 #if MAVLINK_NEED_BYTE_SWAP
00553         union {
00554                 int32_t i;
00555                 uint8_t b[4];
00556         } bin, bout;
00557         bin.i = b;
00558         bout.b[0] = bin.b[3];
00559         bout.b[1] = bin.b[2];
00560         bout.b[2] = bin.b[1];
00561         bout.b[3] = bin.b[0];
00562         v = bout.i;
00563 #else
00564         v = b;
00565 #endif
00566 
00567         // buffer in
00568         // 01100000 01000000 00000000 11110001
00569         // buffer out
00570         // 11110001 00000000 01000000 01100000
00571 
00572         // Existing partly filled byte (four free slots)
00573         // 0111xxxx
00574 
00575         // Mask n free bits
00576         // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
00577         // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
00578 
00579         // Shift n bits into the right position
00580         // out = in >> n;
00581 
00582         // Mask and shift bytes
00583         i_bit_index = bit_index;
00584         i_byte_index = packet_index;
00585         if (bit_index > 0)
00586         {
00587                 // If bits were available at start, they were available
00588                 // in the byte before the current index
00589                 i_byte_index--;
00590         }
00591 
00592         // While bits have not been packed yet
00593         while (bits_remain > 0)
00594         {
00595                 // Bits still have to be packed
00596                 // there can be more than 8 bits, so
00597                 // we might have to pack them into more than one byte
00598 
00599                 // First pack everything we can into the current 'open' byte
00600                 //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
00601                 //FIXME
00602                 if (bits_remain <= (uint8_t)(8 - i_bit_index))
00603                 {
00604                         // Enough space
00605                         curr_bits_n = (uint8_t)bits_remain;
00606                 }
00607                 else
00608                 {
00609                         curr_bits_n = (8 - i_bit_index);
00610                 }
00611                 
00612                 // Pack these n bits into the current byte
00613                 // Mask out whatever was at that position with ones (xxx11111)
00614                 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
00615                 // Put content to this position, by masking out the non-used part
00616                 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
00617                 
00618                 // Increment the bit index
00619                 i_bit_index += curr_bits_n;
00620 
00621                 // Now proceed to the next byte, if necessary
00622                 bits_remain -= curr_bits_n;
00623                 if (bits_remain > 0)
00624                 {
00625                         // Offer another 8 bits / one byte
00626                         i_byte_index++;
00627                         i_bit_index = 0;
00628                 }
00629         }
00630         
00631         *r_bit_index = i_bit_index;
00632         // If a partly filled byte is present, mark this as consumed
00633         if (i_bit_index != 7) i_byte_index++;
00634         return i_byte_index - packet_index;
00635 }
00636 
00637 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00638 
00639 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
00640 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
00641 // whole packet at a time
00642 
00643 /*
00644 
00645 #include "mavlink_types.h"
00646 
00647 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
00648 {
00649     if (chan == MAVLINK_COMM_0)
00650     {
00651         uart0_transmit(ch);
00652     }
00653     if (chan == MAVLINK_COMM_1)
00654     {
00655         uart1_transmit(ch);
00656     }
00657 }
00658  */
00659 
00660 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
00661 {
00662 #ifdef MAVLINK_SEND_UART_BYTES
00663         /* this is the more efficient approach, if the platform
00664            defines it */
00665         MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
00666 #else
00667         /* fallback to one byte at a time */
00668         uint16_t i;
00669         for (i = 0; i < len; i++) {
00670                 comm_send_ch(chan, (uint8_t)buf[i]);
00671         }
00672 #endif
00673 }
00674 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
00675 
00676 #endif /* _MAVLINK_HELPERS_H_ */


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17