protocol.h

00001 #ifndef  _MAVLINK_PROTOCOL_H_
00002 #define  _MAVLINK_PROTOCOL_H_
00003 
00004 #include "string.h"
00005 #include "checksum.h"
00006 
00007 #include "mavlink_types.h"
00008 
00009 extern Serial pc;
00010 
00011 /**
00012  * @brief Initialize the communication stack
00013  *
00014  * This function has to be called before using commParseBuffer() to initialize the different status registers.
00015  *
00016  * @return Will initialize the different buffers and status registers.
00017  */
00018 static void mavlink_parse_state_initialize(mavlink_status_t* initStatus)
00019 {
00020     if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1))
00021     {
00022         initStatus->ck_a = 0;
00023         initStatus->ck_b = 0;
00024         initStatus->msg_received = 0;
00025         initStatus->buffer_overrun = 0;
00026         initStatus->parse_error = 0;
00027         initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT;
00028         initStatus->packet_idx = 0;
00029         initStatus->packet_rx_drop_count = 0;
00030         initStatus->packet_rx_success_count = 0;
00031         initStatus->current_rx_seq = 0;
00032         initStatus->current_tx_seq = 0;
00033     }
00034 }
00035 
00036 static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
00037 {
00038     static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
00039     return &m_mavlink_status[chan];
00040 }
00041 
00042 /**
00043  * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
00044  *
00045  * This function calculates the checksum and sets length and aircraft id correctly.
00046  * It assumes that the message id and the payload are already correctly set. 
00047  *
00048  * @warning This function implicitely assumes the message is sent over channel zero.
00049  *          if the message is sent over a different channel it will reach the receiver
00050  *          without error, BUT the sequence number might be wrong due to the wrong
00051  *          channel sequence counter. This will result is wrongly reported excessive
00052  *          packet loss. Please use @see mavlink_{pack|encode}_headerless and then
00053  *          @see mavlink_finalize_message_chan before sending for a correct channel
00054  *          assignment. Please note that the mavlink_msg_xxx_pack and encode functions
00055  *          assign channel zero as default and thus induce possible loss counter errors.\
00056  *          They have been left to ensure code compatibility.
00057  *
00058  * @see mavlink_finalize_message_chan
00059  * @param msg Message to finalize
00060  * @param system_id Id of the sending (this) system, 1-127
00061  * @param length Message length, usually just the counter incremented while packing the message
00062  */
00063  
00064  
00065 static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length)
00066 {
00067     // This code part is the same for all messages;
00068     //for extra seed in mavlink 1.0
00069     static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
00070     
00071     uint16_t checksum;
00072     msg->len = length;
00073     msg->sysid = system_id;
00074     msg->compid = component_id;
00075     // One sequence number per component
00076     msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq;
00077     mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
00078     checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
00079     //for extra seed in mavlink 1.0
00080     crc_accumulate(mavlink_message_crcs[msg->msgid], &checksum);
00081     
00082     msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
00083     msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
00084 
00085     return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES;
00086 }
00087 
00088 /**
00089  * @brief Finalize a MAVLink message with channel assignment
00090  *
00091  * This function calculates the checksum and sets length and aircraft id correctly.
00092  * It assumes that the message id and the payload are already correctly set. This function
00093  * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
00094  * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
00095  *
00096  * @param msg Message to finalize
00097  * @param system_id Id of the sending (this) system, 1-127
00098  * @param length Message length, usually just the counter incremented while packing the message
00099  */
00100 static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length)
00101 {
00102     // This code part is the same for all messages;
00103     uint16_t checksum;
00104     msg->len = length;
00105     msg->sysid = system_id;
00106     msg->compid = component_id;
00107     // One sequence number per component
00108     msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
00109     mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
00110     checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
00111     msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
00112     msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
00113 
00114     return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES;
00115 }
00116 
00117 /**
00118  * @brief Pack a message to send it over a serial byte stream
00119  */
00120 static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg)
00121 {
00122     *(buffer+0) = MAVLINK_STX; ///< Start transmit
00123     memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload
00124     *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a;
00125     *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b;
00126     return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00127     //return 0;
00128 }
00129 
00130 /**
00131  * @brief Get the required buffer size for this message
00132  */
00133 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
00134 {
00135     return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00136 }
00137 
00138 union checksum_ {
00139     uint16_t s;
00140     uint8_t c[2];
00141 };
00142 
00143 union __mavlink_bitfield {
00144     uint8_t uint8;
00145     int8_t int8;
00146     uint16_t uint16;
00147     int16_t int16;
00148     uint32_t uint32;
00149     int32_t int32;
00150 };
00151 
00152 
00153 static inline void mavlink_start_checksum(mavlink_message_t* msg)
00154 {
00155     union checksum_ ck;
00156     crc_init(&(ck.s));
00157     msg->ck_a = ck.c[0];
00158     msg->ck_b = ck.c[1];
00159 }
00160 
00161 static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
00162 {
00163     union checksum_ ck;
00164     ck.c[0] = msg->ck_a;
00165     ck.c[1] = msg->ck_b;
00166     crc_accumulate(c, &(ck.s));
00167     msg->ck_a = ck.c[0];
00168     msg->ck_b = ck.c[1];
00169 }
00170 
00171 /**
00172  * This is a convenience function which handles the complete MAVLink parsing.
00173  * the function will parse one byte at a time and return the complete packet once
00174  * it could be successfully decoded. Checksum and other failures will be silently
00175  * ignored.
00176  *
00177  * @param chan     ID of the current channel. This allows to parse different channels with this function.
00178  *                 a channel is not a physical message channel like a serial port, but a logic partition of
00179  *                 the communication streams in this case. COMM_NB is the limit for the number of channels
00180  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
00181  * @param c        The char to barse
00182  *
00183  * @param returnMsg NULL if no message could be decoded, the message data else
00184  * @return 0 if no message could be decoded, 1 else
00185  *
00186  * A typical use scenario of this function call is:
00187  *
00188  * @code
00189  * #include <inttypes.h> // For fixed-width uint8_t type
00190  *
00191  * mavlink_message_t msg;
00192  * int chan = 0;
00193  *
00194  *
00195  * while(serial.bytesAvailable > 0)
00196  * {
00197  *   uint8_t byte = serial.getNextByte();
00198  *   if (mavlink_parse_char(chan, byte, &msg))
00199  *     {
00200  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
00201  *     }
00202  * }
00203  *
00204  *
00205  * @endcode
00206  */
00207 static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00208 {
00209     static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
00210 
00211     // For crc calculation with extra seed in mavlink 1.0
00212     //static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
00213 
00214     // Initializes only once, values keep unchanged after first initialization
00215     mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
00216 
00217     mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
00218     mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
00219     int bufferIndex = 0;
00220 
00221     status->msg_received = 0;
00222 
00223     switch (status->parse_state)
00224     {
00225     case MAVLINK_PARSE_STATE_UNINIT:
00226     case MAVLINK_PARSE_STATE_IDLE:
00227         if (c == MAVLINK_STX)
00228         {
00229             status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00230             mavlink_start_checksum(rxmsg);
00231         }
00232         break;
00233 
00234     case MAVLINK_PARSE_STATE_GOT_STX:
00235         if (status->msg_received)
00236         {
00237             status->buffer_overrun++;
00238             status->parse_error++;
00239             status->msg_received = 0;
00240             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00241         }
00242         else
00243         {
00244             // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00245             rxmsg->len = c;
00246             status->packet_idx = 0;
00247             mavlink_update_checksum(rxmsg, c);
00248             status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00249         }
00250         break;
00251 
00252     case MAVLINK_PARSE_STATE_GOT_LENGTH:
00253         rxmsg->seq = c;
00254         mavlink_update_checksum(rxmsg, c);
00255         status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00256         break;
00257 
00258     case MAVLINK_PARSE_STATE_GOT_SEQ:
00259         rxmsg->sysid = c;
00260         mavlink_update_checksum(rxmsg, c);
00261         status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00262         break;
00263 
00264     case MAVLINK_PARSE_STATE_GOT_SYSID:
00265         rxmsg->compid = c;
00266         mavlink_update_checksum(rxmsg, c);
00267         status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00268         break;
00269 
00270     case MAVLINK_PARSE_STATE_GOT_COMPID:
00271         rxmsg->msgid = c;
00272         mavlink_update_checksum(rxmsg, c);
00273         if (rxmsg->len == 0)
00274         {
00275             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00276         }
00277         else
00278         {
00279             status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
00280         }
00281         break;
00282 
00283     case MAVLINK_PARSE_STATE_GOT_MSGID:
00284         rxmsg->payload[status->packet_idx++] = c;
00285         mavlink_update_checksum(rxmsg, c);
00286         if (status->packet_idx == rxmsg->len)
00287         {
00288             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00289         }
00290         break;
00291 
00292     case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00293     /* Add seed to checksum depending on message id */
00294         //mavlink_update_checksum(rxmsg, mavlink_message_crcs[rxmsg->msgid]);
00295         //pc.printf("Calculated Checksum : a %d b %d\n", rxmsg->ck_a, rxmsg->ck_b);
00296         
00297         /*if (c != rxmsg->ck_a)
00298         {
00299             // Check first checksum byte
00300             status->parse_error++;
00301             status->msg_received = 0;
00302             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00303             if (c == MAVLINK_STX)
00304             {
00305                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00306                 mavlink_start_checksum(rxmsg);
00307             }
00308         }
00309         else
00310         {
00311             status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00312         }*/
00313         status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00314         break;
00315 
00316     case MAVLINK_PARSE_STATE_GOT_CRC1:
00317         /*if (c != rxmsg->ck_b)
00318         {// Check second checksum byte
00319             status->parse_error++;
00320             status->msg_received = 0;
00321             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00322             if (c == MAVLINK_STX)
00323             {
00324                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00325                 mavlink_start_checksum(rxmsg);
00326             }
00327         }
00328         else
00329         {
00330             // Successfully got message
00331             status->msg_received = 1;
00332             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00333             memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00334         }*/
00335         status->msg_received = 1;
00336         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00337         memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00338         break;
00339     }
00340 
00341     bufferIndex++;
00342     // If a message has been sucessfully decoded, check index
00343     if (status->msg_received == 1)
00344     {
00345         //while(status->current_seq != rxmsg->seq)
00346         //{
00347         //    status->packet_rx_drop_count++;
00348         //               status->current_seq++;
00349         //}
00350         status->current_rx_seq = rxmsg->seq;
00351         // Initial condition: If no packet has been received so far, drop count is undefined
00352         if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00353         // Count this packet as received
00354         status->packet_rx_success_count++;
00355     }
00356 
00357     r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
00358     r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
00359     r_mavlink_status->packet_rx_drop_count = status->parse_error;
00360     status->parse_error = 0;
00361     return status->msg_received;
00362 }
00363 
00364 
00365 /**
00366  * This is a convenience function which handles the complete MAVLink parsing.
00367  * the function will parse one byte at a time and return the complete packet once
00368  * it could be successfully decoded. Checksum and other failures will be silently
00369  * ignored.
00370  *
00371  * @param chan     ID of the current channel. This allows to parse different channels with this function.
00372  *                 a channel is not a physical message channel like a serial port, but a logic partition of
00373  *                 the communication streams in this case. COMM_NB is the limit for the number of channels
00374  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
00375  * @param c        The char to barse
00376  *
00377  * @param returnMsg NULL if no message could be decoded, the message data else
00378  * @return 0 if no message could be decoded, 1 else
00379  *
00380  * A typical use scenario of this function call is:
00381  *
00382  * @code
00383  * #include <inttypes.h> // For fixed-width uint8_t type
00384  *
00385  * mavlink_message_t msg;
00386  * int chan = 0;
00387  *
00388  *
00389  * while(serial.bytesAvailable > 0)
00390  * {
00391  *   uint8_t byte = serial.getNextByte();
00392  *   if (mavlink_parse_char(chan, byte, &msg))
00393  *     {
00394  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
00395  *     }
00396  * }
00397  *
00398  *
00399  * @endcode
00400  */
00401 
00402 #define MAVLINK_PACKET_START_CANDIDATES 50
00403 /*
00404 static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00405 {
00406         static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
00407         static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2];
00408         static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS];
00409         static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
00410         static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES];
00411         static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS];
00412         static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS];
00413 
00414         // Set a packet start candidate index if sign is start sign
00415         if (c == MAVLINK_STX)
00416         {
00417             m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan];
00418         }
00419 
00420         // Parse normally, if a CRC mismatch occurs retry with the next packet index
00421 }
00422 //    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
00423 //    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
00424 //// Initializes only once, values keep unchanged after first initialization
00425 //    mavlink_parse_state_initialize(&m_mavlink_status[chan]);
00426 //
00427 //mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
00428 //mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status
00429 //int bufferIndex = 0;
00430 //
00431 //status->msg_received = 0;
00432 //
00433 //switch (status->parse_state)
00434 //{
00435 //case MAVLINK_PARSE_STATE_UNINIT:
00436 //case MAVLINK_PARSE_STATE_IDLE:
00437 //            if (c == MAVLINK_STX)
00438 //    {
00439 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00440 //        mavlink_start_checksum(rxmsg);
00441 //    }
00442 //    break;
00443 //
00444 //case MAVLINK_PARSE_STATE_GOT_STX:
00445 //    if (status->msg_received)
00446 //    {
00447 //        status->buffer_overrun++;
00448 //        status->parse_error++;
00449 //        status->msg_received = 0;
00450 //        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00451 //    }
00452 //    else
00453 //    {
00454 //        // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00455 //        rxmsg->len = c;
00456 //        status->packet_idx = 0;
00457 //        mavlink_update_checksum(rxmsg, c);
00458 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00459 //    }
00460 //    break;
00461 //
00462 //case MAVLINK_PARSE_STATE_GOT_LENGTH:
00463 //    rxmsg->seq = c;
00464 //    mavlink_update_checksum(rxmsg, c);
00465 //    status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00466 //    break;
00467 //
00468 //case MAVLINK_PARSE_STATE_GOT_SEQ:
00469 //    rxmsg->sysid = c;
00470 //    mavlink_update_checksum(rxmsg, c);
00471 //    status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00472 //    break;
00473 //
00474 //case MAVLINK_PARSE_STATE_GOT_SYSID:
00475 //    rxmsg->compid = c;
00476 //    mavlink_update_checksum(rxmsg, c);
00477 //    status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00478 //    break;
00479 //
00480 //case MAVLINK_PARSE_STATE_GOT_COMPID:
00481 //    rxmsg->msgid = c;
00482 //    mavlink_update_checksum(rxmsg, c);
00483 //    if (rxmsg->len == 0)
00484 //    {
00485 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00486 //    }
00487 //    else
00488 //    {
00489 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
00490 //    }
00491 //    break;
00492 //
00493 //case MAVLINK_PARSE_STATE_GOT_MSGID:
00494 //    rxmsg->payload[status->packet_idx++] = c;
00495 //    mavlink_update_checksum(rxmsg, c);
00496 //    if (status->packet_idx == rxmsg->len)
00497 //    {
00498 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00499 //    }
00500 //    break;
00501 //
00502 //case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00503 //    if (c != rxmsg->ck_a)
00504 //    {
00505 //        // Check first checksum byte
00506 //        status->parse_error++;
00507 //        status->msg_received = 0;
00508 //        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00509 //    }
00510 //    else
00511 //    {
00512 //        status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00513 //    }
00514 //    break;
00515 //
00516 //case MAVLINK_PARSE_STATE_GOT_CRC1:
00517 //    if (c != rxmsg->ck_b)
00518 //    {// Check second checksum byte
00519 //        status->parse_error++;
00520 //        status->msg_received = 0;
00521 //        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00522 //    }
00523 //    else
00524 //    {
00525 //        // Successfully got message
00526 //        status->msg_received = 1;
00527 //        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00528 //        memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00529 //    }
00530 //    break;
00531 //}
00532 
00533 bufferIndex++;
00534 // If a message has been sucessfully decoded, check index
00535 if (status->msg_received == 1)
00536 {
00537     //while(status->current_seq != rxmsg->seq)
00538     //{
00539     //    status->packet_rx_drop_count++;
00540     //               status->current_seq++;
00541     //}
00542     status->current_seq = rxmsg->seq;
00543     // Initial condition: If no packet has been received so far, drop count is undefined
00544     if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00545     // Count this packet as received
00546     status->packet_rx_success_count++;
00547 }
00548 
00549 r_mavlink_status->current_seq = status->current_seq+1;
00550 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
00551 r_mavlink_status->packet_rx_drop_count = status->parse_error;
00552 return status->msg_received;
00553 }
00554  */
00555 
00556 
00557 typedef union __generic_16bit
00558 {
00559     uint8_t b[2];
00560     int16_t s;
00561 } generic_16bit;
00562 
00563 typedef union __generic_32bit
00564 {
00565     uint8_t b[4];
00566     float f;
00567     int32_t i;
00568     int16_t s;
00569 } generic_32bit;
00570 
00571 typedef union __generic_64bit
00572 {
00573     uint8_t b[8];
00574     int64_t ll; ///< Long long (64 bit)
00575     double  d;  ///< IEEE-754 double precision floating point
00576 } generic_64bit;
00577 
00578 /**
00579  * @brief Place an unsigned byte into the buffer
00580  *
00581  * @param b the byte to add
00582  * @param bindex the position in the packet
00583  * @param buffer the packet buffer
00584  * @return the new position of the last used byte in the buffer
00585  */
00586 static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer)
00587 {
00588     *(buffer + bindex) = b;
00589     return sizeof(b);
00590 }
00591 
00592 /**
00593  * @brief Place a signed byte into the buffer
00594  *
00595  * @param b the byte to add
00596  * @param bindex the position in the packet
00597  * @param buffer the packet buffer
00598  * @return the new position of the last used byte in the buffer
00599  */
00600 static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer)
00601 {
00602     *(buffer + bindex) = (uint8_t)b;
00603     return sizeof(b);
00604 }
00605 
00606 /**
00607  * @brief Place two unsigned bytes into the buffer
00608  *
00609  * @param b the bytes to add
00610  * @param bindex the position in the packet
00611  * @param buffer the packet buffer
00612  * @return the new position of the last used byte in the buffer
00613  */
00614 static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer)
00615 {
00616     buffer[bindex]   = (b>>8)&0xff;
00617     buffer[bindex+1] = (b & 0xff);
00618     return sizeof(b);
00619 }
00620 
00621 /**
00622  * @brief Place two signed bytes into the buffer
00623  *
00624  * @param b the bytes to add
00625  * @param bindex the position in the packet
00626  * @param buffer the packet buffer
00627  * @return the new position of the last used byte in the buffer
00628  */
00629 static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer)
00630 {
00631     return put_uint16_t_by_index(b, bindex, buffer);
00632 }
00633 
00634 /**
00635  * @brief Place four unsigned bytes into the buffer
00636  *
00637  * @param b the bytes to add
00638  * @param bindex the position in the packet
00639  * @param buffer the packet buffer
00640  * @return the new position of the last used byte in the buffer
00641  */
00642 static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer)
00643 {
00644     buffer[bindex]   = (b>>24)&0xff;
00645     buffer[bindex+1] = (b>>16)&0xff;
00646     buffer[bindex+2] = (b>>8)&0xff;
00647     buffer[bindex+3] = (b & 0xff);
00648     return sizeof(b);
00649 }
00650 
00651 /**
00652  * @brief Place four signed bytes into the buffer
00653  *
00654  * @param b the bytes to add
00655  * @param bindex the position in the packet
00656  * @param buffer the packet buffer
00657  * @return the new position of the last used byte in the buffer
00658  */
00659 static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer)
00660 {
00661     buffer[bindex]   = (b>>24)&0xff;
00662     buffer[bindex+1] = (b>>16)&0xff;
00663     buffer[bindex+2] = (b>>8)&0xff;
00664     buffer[bindex+3] = (b & 0xff);
00665     return sizeof(b);
00666 }
00667 
00668 /**
00669  * @brief Place four unsigned bytes into the buffer
00670  *
00671  * @param b the bytes to add
00672  * @param bindex the position in the packet
00673  * @param buffer the packet buffer
00674  * @return the new position of the last used byte in the buffer
00675  */
00676 static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer)
00677 {
00678     buffer[bindex]   = (b>>56)&0xff;
00679     buffer[bindex+1] = (b>>48)&0xff;
00680     buffer[bindex+2] = (b>>40)&0xff;
00681     buffer[bindex+3] = (b>>32)&0xff;
00682     buffer[bindex+4] = (b>>24)&0xff;
00683     buffer[bindex+5] = (b>>16)&0xff;
00684     buffer[bindex+6] = (b>>8)&0xff;
00685     buffer[bindex+7] = (b & 0xff);
00686     return sizeof(b);
00687 }
00688 
00689 /**
00690  * @brief Place four signed bytes into the buffer
00691  *
00692  * @param b the bytes to add
00693  * @param bindex the position in the packet
00694  * @param buffer the packet buffer
00695  * @return the new position of the last used byte in the buffer
00696  */
00697 static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer)
00698 {
00699     return put_uint64_t_by_index(b, bindex, buffer);
00700 }
00701 
00702 /**
00703  * @brief Place a float into the buffer
00704  *
00705  * @param b the float to add
00706  * @param bindex the position in the packet
00707  * @param buffer the packet buffer
00708  * @return the new position of the last used byte in the buffer
00709  */
00710 static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer)
00711 {
00712     generic_32bit g;
00713     g.f = b;
00714     return put_int32_t_by_index(g.i, bindex, buffer);
00715 }
00716 
00717 /**
00718  * @brief Place a double into the buffer
00719  *
00720  * @param b the double to add
00721  * @param bindex the position in the packet
00722  * @param buffer the packet buffer
00723  * @return the new position of the last used byte in the buffer
00724  */
00725 static inline uint8_t put_double_by_index(double b, uint8_t bindex, uint8_t* buffer)
00726 {
00727     generic_64bit g;
00728     g.d = b;
00729     return put_int64_t_by_index(g.ll, bindex, buffer);
00730 }
00731 
00732 /**
00733  * @brief Place an array into the buffer
00734  *
00735  * @param b the array to add
00736  * @param length size of the array (for strings: length WITH '\0' char)
00737  * @param bindex the position in the packet
00738  * @param buffer packet buffer
00739  * @return new position of the last used byte in the buffer
00740  */
00741 static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer)
00742 {
00743     memcpy(buffer+bindex, b, length);
00744     return length;
00745 }
00746 
00747 /**
00748  * @brief Place a string into the buffer
00749  *
00750  * @param b the string to add
00751  * @param maxlength size of the array (for strings: length WITHOUT '\0' char)
00752  * @param bindex the position in the packet
00753  * @param buffer packet buffer
00754  * @return new position of the last used byte in the buffer
00755  */
00756 static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer)
00757 {
00758     uint16_t length = 0;
00759     // Copy string into buffer, ensuring not to exceed the buffer size
00760     int i;
00761     for (i = 1; i < maxlength; i++)
00762     {
00763         length++;
00764         // String characters
00765         if (i < (maxlength - 1))
00766         {
00767             buffer[bindex+i] = b[i];
00768             // Stop at null character
00769             if (b[i] == '\0')
00770             {
00771                 break;
00772             }
00773         }
00774         // Enforce null termination at end of buffer
00775         else if (i == (maxlength - 1))
00776         {
00777             buffer[i] = '\0';
00778         }
00779     }
00780     // Write length into first field
00781     put_uint8_t_by_index(length, bindex, buffer);
00782     return length;
00783 }
00784 
00785 /**
00786  * @brief Put a bitfield of length 1-32 bit into the buffer
00787  *
00788  * @param b the value to add, will be encoded in the bitfield
00789  * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
00790  * @param packet_index the position in the packet (the index of the first byte to use)
00791  * @param bit_index the position in the byte (the index of the first bit to use)
00792  * @param buffer packet buffer to write into
00793  * @return new position of the last used byte in the buffer
00794  */
00795 static inline 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)
00796 {
00797     uint16_t bits_remain = bits;
00798     // Transform number into network order
00799     generic_32bit bin;
00800     generic_32bit bout;
00801     uint8_t i_bit_index, i_byte_index, curr_bits_n;
00802     bin.i = b;
00803     bout.b[0] = bin.b[3];
00804     bout.b[1] = bin.b[2];
00805     bout.b[2] = bin.b[1];
00806     bout.b[3] = bin.b[0];
00807 
00808     // buffer in
00809     // 01100000 01000000 00000000 11110001
00810     // buffer out
00811     // 11110001 00000000 01000000 01100000
00812 
00813     // Existing partly filled byte (four free slots)
00814     // 0111xxxx
00815 
00816     // Mask n free bits
00817     // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
00818     // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
00819 
00820     // Shift n bits into the right position
00821     // out = in >> n;
00822 
00823     // Mask and shift bytes
00824     i_bit_index = bit_index;
00825     i_byte_index = packet_index;
00826     if (bit_index > 0)
00827     {
00828         // If bits were available at start, they were available
00829         // in the byte before the current index
00830         i_byte_index--;
00831     }
00832 
00833     // While bits have not been packed yet
00834     while (bits_remain > 0)
00835     {
00836         // Bits still have to be packed
00837         // there can be more than 8 bits, so
00838         // we might have to pack them into more than one byte
00839 
00840         // First pack everything we can into the current 'open' byte
00841         //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
00842         //FIXME
00843         if (bits_remain <= (8 - i_bit_index))
00844         {
00845             // Enough space
00846             curr_bits_n = bits_remain;
00847         }
00848         else
00849         {
00850             curr_bits_n = (8 - i_bit_index);
00851         }
00852         
00853         // Pack these n bits into the current byte
00854         // Mask out whatever was at that position with ones (xxx11111)
00855         buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
00856         // Put content to this position, by masking out the non-used part
00857         buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i);
00858         
00859         // Increment the bit index
00860         i_bit_index += curr_bits_n;
00861 
00862         // Now proceed to the next byte, if necessary
00863         bits_remain -= curr_bits_n;
00864         if (bits_remain > 0)
00865         {
00866             // Offer another 8 bits / one byte
00867             i_byte_index++;
00868             i_bit_index = 0;
00869         }
00870     }
00871     
00872     *r_bit_index = i_bit_index;
00873     // If a partly filled byte is present, mark this as consumed
00874     if (i_bit_index != 7) i_byte_index++;
00875     return i_byte_index - packet_index;
00876 }
00877 
00878 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00879 
00880 // To make MAVLink work on your MCU, define a similar function
00881 
00882 /*
00883 
00884 #include "mavlink_types.h"
00885 
00886 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
00887 {
00888     if (chan == MAVLINK_COMM_0)
00889     {
00890         uart0_transmit(ch);
00891     }
00892     if (chan == MAVLINK_COMM_1)
00893     {
00894         uart1_transmit(ch);
00895     }
00896 }
00897  */
00898 
00899 static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum)
00900 {
00901     crc_accumulate(b, checksum);
00902     comm_send_ch(chan, b);
00903 }
00904 
00905 static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum)
00906 {
00907     crc_accumulate(b, checksum);
00908     comm_send_ch(chan, b);
00909 }
00910 
00911 static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum)
00912 {
00913     char s;
00914     s = (b>>8)&0xff;
00915     comm_send_ch(chan, s);
00916     crc_accumulate(s, checksum);
00917     s = (b & 0xff);
00918     comm_send_ch(chan, s);
00919     crc_accumulate(s, checksum);
00920 }
00921 
00922 static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum)
00923 {
00924     mavlink_send_uart_uint16_t(chan, b, checksum);
00925 }
00926 
00927 static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum)
00928 {
00929     char s;
00930     s = (b>>24)&0xff;
00931     comm_send_ch(chan, s);
00932     crc_accumulate(s, checksum);
00933     s = (b>>16)&0xff;
00934     comm_send_ch(chan, s);
00935     crc_accumulate(s, checksum);
00936     s = (b>>8)&0xff;
00937     comm_send_ch(chan, s);
00938     crc_accumulate(s, checksum);
00939     s = (b & 0xff);
00940     comm_send_ch(chan, s);
00941     crc_accumulate(s, checksum);
00942 }
00943 
00944 static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum)
00945 {
00946     mavlink_send_uart_uint32_t(chan, b, checksum);
00947 }
00948 
00949 static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum)
00950 {
00951     char s;
00952     s = (b>>56)&0xff;
00953     comm_send_ch(chan, s);
00954     crc_accumulate(s, checksum);
00955     s = (b>>48)&0xff;
00956     comm_send_ch(chan, s);
00957     crc_accumulate(s, checksum);
00958     s = (b>>40)&0xff;
00959     comm_send_ch(chan, s);
00960     crc_accumulate(s, checksum);
00961     s = (b>>32)&0xff;
00962     comm_send_ch(chan, s);
00963     crc_accumulate(s, checksum);
00964     s = (b>>24)&0xff;
00965     comm_send_ch(chan, s);
00966     crc_accumulate(s, checksum);
00967     s = (b>>16)&0xff;
00968     comm_send_ch(chan, s);
00969     crc_accumulate(s, checksum);
00970     s = (b>>8)&0xff;
00971     comm_send_ch(chan, s);
00972     crc_accumulate(s, checksum);
00973     s = (b & 0xff);
00974     comm_send_ch(chan, s);
00975     crc_accumulate(s, checksum);
00976 }
00977 
00978 static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum)
00979 {
00980     mavlink_send_uart_uint64_t(chan, b, checksum);
00981 }
00982 
00983 static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum)
00984 {
00985     generic_32bit g;
00986     g.f = b;
00987     mavlink_send_uart_uint32_t(chan, g.i, checksum);
00988 }
00989 
00990 static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum)
00991 {
00992     generic_64bit g;
00993     g.d = b;
00994     mavlink_send_uart_uint32_t(chan, g.ll, checksum);
00995 }
00996 
00997 static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg)
00998 {
00999     // ARM7 MCU board implementation
01000     // Create pointer on message struct
01001     // Send STX
01002     comm_send_ch(chan, MAVLINK_STX);
01003     comm_send_ch(chan, msg->len);
01004     comm_send_ch(chan, msg->seq);
01005     comm_send_ch(chan, msg->sysid);
01006     comm_send_ch(chan, msg->compid);
01007     comm_send_ch(chan, msg->msgid);
01008     for(uint16_t i = 0; i < msg->len; i++)
01009     {
01010         comm_send_ch(chan, msg->payload[i]);
01011     }
01012     comm_send_ch(chan, msg->ck_a);
01013     comm_send_ch(chan, msg->ck_b);
01014 }
01015 #endif
01016 
01017 #endif /* _MAVLINK_PROTOCOL_H_ */