mavlink_helpers.h

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 
00046 /**
00047  * @brief Reset the status of a channel.
00048  */
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 
00055 /**
00056  * @brief Finalize a MAVLink message with channel assignment
00057  *
00058  * This function calculates the checksum and sets length and aircraft id correctly.
00059  * It assumes that the message id and the payload are already correctly set. This function
00060  * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
00061  * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
00062  *
00063  * @param msg Message to finalize
00064  * @param system_id Id of the sending (this) system, 1-127
00065  * @param length Message length
00066  */
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 min_length, 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 is only used for the v2 protocol and we silence it here
00076     (void)min_length;
00077     // This code part is the same for all messages;
00078     msg->magic = MAVLINK_STX;
00079     msg->len = length;
00080     msg->sysid = system_id;
00081     msg->compid = component_id;
00082     // One sequence number per channel
00083     msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
00084     mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
00085     msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
00086     crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
00087 #if MAVLINK_CRC_EXTRA
00088     crc_accumulate(crc_extra, &msg->checksum);
00089 #endif
00090     mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
00091     mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
00092 
00093     return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00094 }
00095 
00096 
00097 /**
00098  * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
00099  */
00100 #if MAVLINK_CRC_EXTRA
00101 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00102                          uint8_t min_length, uint8_t length, uint8_t crc_extra)
00103 {
00104     return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
00105 }
00106 #else
00107 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00108                          uint8_t length)
00109 {
00110     return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
00111 }
00112 #endif
00113 
00114 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00115 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
00116 
00117 /**
00118  * @brief Finalize a MAVLink message with channel assignment and send
00119  */
00120 #if MAVLINK_CRC_EXTRA
00121 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
00122                             uint8_t min_length, uint8_t length, uint8_t crc_extra)
00123 #else
00124 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
00125 #endif
00126 {
00127     uint16_t checksum;
00128     uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
00129     uint8_t ck[2];
00130     mavlink_status_t *status = mavlink_get_channel_status(chan);
00131     buf[0] = MAVLINK_STX;
00132     buf[1] = length;
00133     buf[2] = status->current_tx_seq;
00134     buf[3] = mavlink_system.sysid;
00135     buf[4] = mavlink_system.compid;
00136     buf[5] = msgid;
00137     status->current_tx_seq++;
00138     checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
00139     crc_accumulate_buffer(&checksum, packet, length);
00140 #if MAVLINK_CRC_EXTRA
00141     crc_accumulate(crc_extra, &checksum);
00142 #endif
00143     ck[0] = (uint8_t)(checksum & 0xFF);
00144     ck[1] = (uint8_t)(checksum >> 8);
00145 
00146     MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
00147     _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
00148     _mavlink_send_uart(chan, packet, length);
00149     _mavlink_send_uart(chan, (const char *)ck, 2);
00150     MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
00151 }
00152 
00153 /**
00154  * @brief re-send a message over a uart channel
00155  * this is more stack efficient than re-marshalling the message
00156  */
00157 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
00158 {
00159     uint8_t ck[2];
00160 
00161     ck[0] = (uint8_t)(msg->checksum & 0xFF);
00162     ck[1] = (uint8_t)(msg->checksum >> 8);
00163     // XXX use the right sequence here
00164 
00165     MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
00166     _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
00167     _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
00168     _mavlink_send_uart(chan, (const char *)ck, 2);
00169     MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
00170 }
00171 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
00172 
00173 /**
00174  * @brief Pack a message to send it over a serial byte stream
00175  */
00176 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
00177 {
00178     memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
00179 
00180     uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
00181 
00182     ck[0] = (uint8_t)(msg->checksum & 0xFF);
00183     ck[1] = (uint8_t)(msg->checksum >> 8);
00184 
00185     return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
00186 }
00187 
00188 union __mavlink_bitfield {
00189     uint8_t uint8;
00190     int8_t int8;
00191     uint16_t uint16;
00192     int16_t int16;
00193     uint32_t uint32;
00194     int32_t int32;
00195 };
00196 
00197 
00198 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
00199 {
00200     crc_init(&msg->checksum);
00201 }
00202 
00203 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
00204 {
00205     crc_accumulate(c, &msg->checksum);
00206 }
00207 
00208 /**
00209  * This is a varient of mavlink_frame_char() but with caller supplied
00210  * parsing buffers. It is useful when you want to create a MAVLink
00211  * parser in a library that doesn't use any global variables
00212  *
00213  * @param rxmsg    parsing message buffer
00214  * @param status   parsing starus buffer 
00215  * @param c        The char to parse
00216  *
00217  * @param returnMsg NULL if no message could be decoded, the message data else
00218  * @param returnStats if a message was decoded, this is filled with the channel's stats
00219  * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
00220  *
00221  * A typical use scenario of this function call is:
00222  *
00223  * @code
00224  * #include <mavlink.h>
00225  *
00226  * mavlink_message_t msg;
00227  * int chan = 0;
00228  *
00229  *
00230  * while(serial.bytesAvailable > 0)
00231  * {
00232  *   uint8_t byte = serial.getNextByte();
00233  *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
00234  *     {
00235  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
00236  *     }
00237  * }
00238  *
00239  *
00240  * @endcode
00241  */
00242 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
00243                                                  mavlink_status_t* status,
00244                                                  uint8_t c, 
00245                                                  mavlink_message_t* r_message, 
00246                                                  mavlink_status_t* r_mavlink_status)
00247 {
00248         /*
00249       default message crc function. You can override this per-system to
00250       put this data in a different memory segment
00251     */
00252 #if MAVLINK_CRC_EXTRA
00253 #ifndef MAVLINK_MESSAGE_CRC
00254     static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
00255 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
00256 #endif
00257 #endif
00258 
00259     /* Enable this option to check the length of each message.
00260        This allows invalid messages to be caught much sooner. Use if the transmission
00261        medium is prone to missing (or extra) characters (e.g. a radio that fades in
00262        and out). Only use if the channel will only contain messages types listed in
00263        the headers.
00264     */
00265 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00266 #ifndef MAVLINK_MESSAGE_LENGTH
00267     static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
00268 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
00269 #endif
00270 #endif
00271 
00272     int bufferIndex = 0;
00273 
00274     status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00275 
00276     switch (status->parse_state)
00277     {
00278     case MAVLINK_PARSE_STATE_UNINIT:
00279     case MAVLINK_PARSE_STATE_IDLE:
00280         if (c == MAVLINK_STX)
00281         {
00282             status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00283             rxmsg->len = 0;
00284             rxmsg->magic = c;
00285             mavlink_start_checksum(rxmsg);
00286         }
00287         break;
00288 
00289     case MAVLINK_PARSE_STATE_GOT_STX:
00290             if (status->msg_received 
00291 /* Support shorter buffers than the
00292    default maximum packet size */
00293 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
00294                 || c > MAVLINK_MAX_PAYLOAD_LEN
00295 #endif
00296                 )
00297         {
00298             status->buffer_overrun++;
00299             status->parse_error++;
00300             status->msg_received = 0;
00301             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00302         }
00303         else
00304         {
00305             // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00306             rxmsg->len = c;
00307             status->packet_idx = 0;
00308             mavlink_update_checksum(rxmsg, c);
00309             status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00310         }
00311         break;
00312 
00313     case MAVLINK_PARSE_STATE_GOT_LENGTH:
00314         rxmsg->seq = c;
00315         mavlink_update_checksum(rxmsg, c);
00316         status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00317         break;
00318 
00319     case MAVLINK_PARSE_STATE_GOT_SEQ:
00320         rxmsg->sysid = c;
00321         mavlink_update_checksum(rxmsg, c);
00322         status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00323         break;
00324 
00325     case MAVLINK_PARSE_STATE_GOT_SYSID:
00326         rxmsg->compid = c;
00327         mavlink_update_checksum(rxmsg, c);
00328         status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00329         break;
00330 
00331     case MAVLINK_PARSE_STATE_GOT_COMPID:
00332 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00333             if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
00334         {
00335             status->parse_error++;
00336             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00337             break;
00338         }
00339 #endif
00340         rxmsg->msgid = c;
00341         mavlink_update_checksum(rxmsg, c);
00342         if (rxmsg->len == 0)
00343         {
00344             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00345         }
00346         else
00347         {
00348             status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
00349         }
00350         break;
00351 
00352     case MAVLINK_PARSE_STATE_GOT_MSGID:
00353         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
00354         mavlink_update_checksum(rxmsg, c);
00355         if (status->packet_idx == rxmsg->len)
00356         {
00357             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00358         }
00359         break;
00360 
00361     case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00362 #if MAVLINK_CRC_EXTRA
00363         mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
00364 #endif
00365         if (c != (rxmsg->checksum & 0xFF)) {
00366             status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
00367         } else {
00368             status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00369         }
00370                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
00371         break;
00372 
00373     case MAVLINK_PARSE_STATE_GOT_CRC1:
00374     case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
00375         if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
00376             // got a bad CRC message
00377             status->msg_received = MAVLINK_FRAMING_BAD_CRC;
00378         } else {
00379             // Successfully got message
00380             status->msg_received = MAVLINK_FRAMING_OK;
00381                 }
00382                 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00383                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
00384                 memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00385         break;
00386     }
00387 
00388     bufferIndex++;
00389     // If a message has been sucessfully decoded, check index
00390     if (status->msg_received == MAVLINK_FRAMING_OK)
00391     {
00392         //while(status->current_seq != rxmsg->seq)
00393         //{
00394         //  status->packet_rx_drop_count++;
00395         //               status->current_seq++;
00396         //}
00397         status->current_rx_seq = rxmsg->seq;
00398         // Initial condition: If no packet has been received so far, drop count is undefined
00399         if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00400         // Count this packet as received
00401         status->packet_rx_success_count++;
00402     }
00403 
00404     r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
00405     r_mavlink_status->parse_state = status->parse_state;
00406     r_mavlink_status->packet_idx = status->packet_idx;
00407     r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
00408     r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
00409     r_mavlink_status->packet_rx_drop_count = status->parse_error;
00410     status->parse_error = 0;
00411 
00412     if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
00413         /*
00414           the CRC came out wrong. We now need to overwrite the
00415           msg CRC with the one on the wire so that if the
00416           caller decides to forward the message anyway that
00417           mavlink_msg_to_send_buffer() won't overwrite the
00418           checksum
00419          */
00420         r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
00421     }
00422 
00423     return status->msg_received;
00424 }
00425 
00426 /**
00427  * This is a convenience function which handles the complete MAVLink parsing.
00428  * the function will parse one byte at a time and return the complete packet once
00429  * it could be successfully decoded. This function will return 0, 1 or
00430  * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
00431  *
00432  * Messages are parsed into an internal buffer (one for each channel). When a complete
00433  * message is received it is copies into *returnMsg and the channel's status is
00434  * copied into *returnStats.
00435  *
00436  * @param chan     ID of the current channel. This allows to parse different channels with this function.
00437  *                 a channel is not a physical message channel like a serial port, but a logic partition of
00438  *                 the communication streams in this case. COMM_NB is the limit for the number of channels
00439  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
00440  * @param c        The char to parse
00441  *
00442  * @param returnMsg NULL if no message could be decoded, the message data else
00443  * @param returnStats if a message was decoded, this is filled with the channel's stats
00444  * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
00445  *
00446  * A typical use scenario of this function call is:
00447  *
00448  * @code
00449  * #include <mavlink.h>
00450  *
00451  * mavlink_message_t msg;
00452  * int chan = 0;
00453  *
00454  *
00455  * while(serial.bytesAvailable > 0)
00456  * {
00457  *   uint8_t byte = serial.getNextByte();
00458  *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
00459  *     {
00460  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
00461  *     }
00462  * }
00463  *
00464  *
00465  * @endcode
00466  */
00467 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00468 {
00469     return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
00470                      mavlink_get_channel_status(chan),
00471                      c,
00472                      r_message,
00473                      r_mavlink_status);
00474 }
00475 
00476 
00477 /**
00478  * This is a convenience function which handles the complete MAVLink parsing.
00479  * the function will parse one byte at a time and return the complete packet once
00480  * it could be successfully decoded. This function will return 0 or 1.
00481  *
00482  * Messages are parsed into an internal buffer (one for each channel). When a complete
00483  * message is received it is copies into *returnMsg and the channel's status is
00484  * copied into *returnStats.
00485  *
00486  * @param chan     ID of the current channel. This allows to parse different channels with this function.
00487  *                 a channel is not a physical message channel like a serial port, but a logic partition of
00488  *                 the communication streams in this case. COMM_NB is the limit for the number of channels
00489  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
00490  * @param c        The char to parse
00491  *
00492  * @param returnMsg NULL if no message could be decoded, the message data else
00493  * @param returnStats if a message was decoded, this is filled with the channel's stats
00494  * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
00495  *
00496  * A typical use scenario of this function call is:
00497  *
00498  * @code
00499  * #include <mavlink.h>
00500  *
00501  * mavlink_message_t msg;
00502  * int chan = 0;
00503  *
00504  *
00505  * while(serial.bytesAvailable > 0)
00506  * {
00507  *   uint8_t byte = serial.getNextByte();
00508  *   if (mavlink_parse_char(chan, byte, &msg))
00509  *     {
00510  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
00511  *     }
00512  * }
00513  *
00514  *
00515  * @endcode
00516  */
00517 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00518 {
00519     uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
00520     if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
00521         // we got a bad CRC. Treat as a parse failure
00522         mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
00523         mavlink_status_t* status = mavlink_get_channel_status(chan);
00524         status->parse_error++;
00525         status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00526         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00527         if (c == MAVLINK_STX)
00528         {
00529             status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00530             rxmsg->len = 0;
00531             mavlink_start_checksum(rxmsg);
00532         }
00533         return 0;
00534     }
00535     return msg_received;
00536 }
00537 
00538 /**
00539  * @brief Put a bitfield of length 1-32 bit into the buffer
00540  *
00541  * @param b the value to add, will be encoded in the bitfield
00542  * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
00543  * @param packet_index the position in the packet (the index of the first byte to use)
00544  * @param bit_index the position in the byte (the index of the first bit to use)
00545  * @param buffer packet buffer to write into
00546  * @return new position of the last used byte in the buffer
00547  */
00548 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)
00549 {
00550     uint16_t bits_remain = bits;
00551     // Transform number into network order
00552     int32_t v;
00553     uint8_t i_bit_index, i_byte_index, curr_bits_n;
00554 #if MAVLINK_NEED_BYTE_SWAP
00555     union {
00556         int32_t i;
00557         uint8_t b[4];
00558     } bin, bout;
00559     bin.i = b;
00560     bout.b[0] = bin.b[3];
00561     bout.b[1] = bin.b[2];
00562     bout.b[2] = bin.b[1];
00563     bout.b[3] = bin.b[0];
00564     v = bout.i;
00565 #else
00566     v = b;
00567 #endif
00568 
00569     // buffer in
00570     // 01100000 01000000 00000000 11110001
00571     // buffer out
00572     // 11110001 00000000 01000000 01100000
00573 
00574     // Existing partly filled byte (four free slots)
00575     // 0111xxxx
00576 
00577     // Mask n free bits
00578     // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
00579     // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
00580 
00581     // Shift n bits into the right position
00582     // out = in >> n;
00583 
00584     // Mask and shift bytes
00585     i_bit_index = bit_index;
00586     i_byte_index = packet_index;
00587     if (bit_index > 0)
00588     {
00589         // If bits were available at start, they were available
00590         // in the byte before the current index
00591         i_byte_index--;
00592     }
00593 
00594     // While bits have not been packed yet
00595     while (bits_remain > 0)
00596     {
00597         // Bits still have to be packed
00598         // there can be more than 8 bits, so
00599         // we might have to pack them into more than one byte
00600 
00601         // First pack everything we can into the current 'open' byte
00602         //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
00603         //FIXME
00604         if (bits_remain <= (uint8_t)(8 - i_bit_index))
00605         {
00606             // Enough space
00607             curr_bits_n = (uint8_t)bits_remain;
00608         }
00609         else
00610         {
00611             curr_bits_n = (8 - i_bit_index);
00612         }
00613         
00614         // Pack these n bits into the current byte
00615         // Mask out whatever was at that position with ones (xxx11111)
00616         buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
00617         // Put content to this position, by masking out the non-used part
00618         buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
00619         
00620         // Increment the bit index
00621         i_bit_index += curr_bits_n;
00622 
00623         // Now proceed to the next byte, if necessary
00624         bits_remain -= curr_bits_n;
00625         if (bits_remain > 0)
00626         {
00627             // Offer another 8 bits / one byte
00628             i_byte_index++;
00629             i_bit_index = 0;
00630         }
00631     }
00632     
00633     *r_bit_index = i_bit_index;
00634     // If a partly filled byte is present, mark this as consumed
00635     if (i_bit_index != 7) i_byte_index++;
00636     return i_byte_index - packet_index;
00637 }
00638 
00639 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00640 
00641 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
00642 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
00643 // whole packet at a time
00644 
00645 /*
00646 
00647 #include "mavlink_types.h"
00648 
00649 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
00650 {
00651     if (chan == MAVLINK_COMM_0)
00652     {
00653         uart0_transmit(ch);
00654     }
00655     if (chan == MAVLINK_COMM_1)
00656     {
00657         uart1_transmit(ch);
00658     }
00659 }
00660  */
00661 
00662 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
00663 {
00664 #ifdef MAVLINK_SEND_UART_BYTES
00665     /* this is the more efficient approach, if the platform
00666        defines it */
00667     MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
00668 #else
00669     /* fallback to one byte at a time */
00670     uint16_t i;
00671     for (i = 0; i < len; i++) {
00672         comm_send_ch(chan, (uint8_t)buf[i]);
00673     }
00674 #endif
00675 }
00676 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
00677 
00678 #endif /* _MAVLINK_HELPERS_H_ */
00679