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 #include <stdio.h>
00009 
00010 #ifndef MAVLINK_HELPER
00011 #define MAVLINK_HELPER
00012 #endif
00013 
00014 #include "mavlink_sha256.h"
00015 
00016 /*
00017  * Internal function to give access to the channel status for each channel
00018  */
00019 #ifndef MAVLINK_GET_CHANNEL_STATUS
00020 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
00021 {
00022 #ifdef MAVLINK_EXTERNAL_RX_STATUS
00023         // No m_mavlink_status array defined in function,
00024         // has to be defined externally
00025 #else
00026         static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
00027 #endif
00028         return &m_mavlink_status[chan];
00029 }
00030 #endif
00031 
00032 /*
00033  * Internal function to give access to the channel buffer for each channel
00034  */
00035 #ifndef MAVLINK_GET_CHANNEL_BUFFER
00036 MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
00037 {
00038         
00039 #ifdef MAVLINK_EXTERNAL_RX_BUFFER
00040         // No m_mavlink_buffer array defined in function,
00041         // has to be defined externally
00042 #else
00043         static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
00044 #endif
00045         return &m_mavlink_buffer[chan];
00046 }
00047 #endif // MAVLINK_GET_CHANNEL_BUFFER
00048 
00052 MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
00053 {
00054         mavlink_status_t *status = mavlink_get_channel_status(chan);
00055         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00056 }
00057 
00061 MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
00062                                            uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
00063                                            const uint8_t *header, uint8_t header_len,
00064                                            const uint8_t *packet, uint8_t packet_len,
00065                                            const uint8_t crc[2])
00066 {
00067         mavlink_sha256_ctx ctx;
00068         union {
00069             uint64_t t64;
00070             uint8_t t8[8];
00071         } tstamp;
00072         if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
00073             return 0;
00074         }
00075         signature[0] = signing->link_id;
00076         tstamp.t64 = signing->timestamp;
00077         memcpy(&signature[1], tstamp.t8, 6);
00078         signing->timestamp++;
00079         
00080         mavlink_sha256_init(&ctx);
00081         mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
00082         mavlink_sha256_update(&ctx, header, header_len);
00083         mavlink_sha256_update(&ctx, packet, packet_len);
00084         mavlink_sha256_update(&ctx, crc, 2);
00085         mavlink_sha256_update(&ctx, signature, 7);
00086         mavlink_sha256_final_48(&ctx, &signature[7]);
00087         
00088         return MAVLINK_SIGNATURE_BLOCK_LEN;
00089 }
00090 
00094 MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
00095                                             mavlink_signing_streams_t *signing_streams,
00096                                             const mavlink_message_t *msg)
00097 {
00098         if (signing == NULL) {
00099                 return true;
00100         }
00101         const uint8_t *p = (const uint8_t *)&msg->magic;
00102         uint16_t len = MAVLINK_CORE_HEADER_LEN+1+msg->len+2+1+6;
00103         const uint8_t *psig = p + MAVLINK_CORE_HEADER_LEN+1+msg->len+2;
00104         const uint8_t *incoming_signature = psig+7;
00105         mavlink_sha256_ctx ctx;
00106         uint8_t signature[6];
00107         uint16_t i;
00108         
00109         mavlink_sha256_init(&ctx);
00110         mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
00111         mavlink_sha256_update(&ctx, p, len);
00112         mavlink_sha256_final_48(&ctx, signature);
00113         if (memcmp(signature, incoming_signature, 6) != 0) {
00114                 return false;
00115         }
00116 
00117         // now check timestamp
00118         union tstamp {
00119             uint64_t t64;
00120             uint8_t t8[8];
00121         } tstamp;
00122         uint8_t link_id = psig[0];
00123         tstamp.t64 = 0;
00124         memcpy(tstamp.t8, psig+1, 6);
00125 
00126         if (signing_streams == NULL) {
00127                 return false;
00128         }
00129         
00130         // find stream
00131         for (i=0; i<signing_streams->num_signing_streams; i++) {
00132                 if (msg->sysid == signing_streams->stream[i].sysid &&
00133                     msg->compid == signing_streams->stream[i].compid &&
00134                     link_id == signing_streams->stream[i].link_id) {
00135                         break;
00136                 }
00137         }
00138         if (i == signing_streams->num_signing_streams) {
00139                 if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
00140                         // over max number of streams
00141                         return false;
00142                 }
00143                 // new stream. Only accept if timestamp is not more than 1 minute old
00144                 if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
00145                         return false;
00146                 }
00147                 // add new stream
00148                 signing_streams->stream[i].sysid = msg->sysid;
00149                 signing_streams->stream[i].compid = msg->compid;
00150                 signing_streams->stream[i].link_id = link_id;
00151                 signing_streams->num_signing_streams++;
00152         } else {
00153                 union tstamp last_tstamp;
00154                 last_tstamp.t64 = 0;
00155                 memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
00156                 if (tstamp.t64 <= last_tstamp.t64) {
00157                         // repeating old timestamp
00158                         return false;
00159                 }
00160         }
00161 
00162         // remember last timestamp
00163         memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
00164 
00165         // our next timestamp must be at least this timestamp
00166         if (tstamp.t64 > signing->timestamp) {
00167                 signing->timestamp = tstamp.t64;
00168         }
00169         return true;
00170 }
00171 
00172 
00185 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00186                                                       uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
00187 {
00188         mavlink_status_t *status = mavlink_get_channel_status(chan);
00189         bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
00190         bool signing =  (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
00191         uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
00192         uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
00193         uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
00194         if (mavlink1) {
00195                 msg->magic = MAVLINK_STX_MAVLINK1;
00196                 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
00197         } else {
00198                 msg->magic = MAVLINK_STX;
00199         }
00200         msg->len = mavlink1?min_length:length;
00201         msg->sysid = system_id;
00202         msg->compid = component_id;
00203         msg->incompat_flags = 0;
00204         if (signing) {
00205                 msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
00206         }
00207         msg->compat_flags = 0;
00208         msg->seq = status->current_tx_seq;
00209         status->current_tx_seq = status->current_tx_seq + 1;
00210 
00211         // form the header as a byte array for the crc
00212         buf[0] = msg->magic;
00213         buf[1] = msg->len;
00214         if (mavlink1) {
00215                 buf[2] = msg->seq;
00216                 buf[3] = msg->sysid;
00217                 buf[4] = msg->compid;
00218                 buf[5] = msg->msgid & 0xFF;
00219         } else {
00220                 buf[2] = msg->incompat_flags;
00221                 buf[3] = msg->compat_flags;
00222                 buf[4] = msg->seq;
00223                 buf[5] = msg->sysid;
00224                 buf[6] = msg->compid;
00225                 buf[7] = msg->msgid & 0xFF;
00226                 buf[8] = (msg->msgid >> 8) & 0xFF;
00227                 buf[9] = (msg->msgid >> 16) & 0xFF;
00228         }
00229         
00230         msg->checksum = crc_calculate(&buf[1], header_len-1);
00231         crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
00232         crc_accumulate(crc_extra, &msg->checksum);
00233         mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
00234         mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
00235 
00236         if (signing) {
00237                 mavlink_sign_packet(status->signing,
00238                                     msg->signature,
00239                                     (const uint8_t *)buf, header_len,
00240                                     (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
00241                                     (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
00242         }
00243         
00244         return msg->len + header_len + 2 + signature_len;
00245 }
00246 
00247 
00251 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00252                                                  uint8_t min_length, uint8_t length, uint8_t crc_extra)
00253 {
00254     return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
00255 }
00256 
00257 static inline void _mav_parse_error(mavlink_status_t *status)
00258 {
00259     status->parse_error++;
00260 }
00261 
00262 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00263 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
00264 
00268 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
00269                                                     const char *packet, 
00270                                                     uint8_t min_length, uint8_t length, uint8_t crc_extra)
00271 {
00272         uint16_t checksum;
00273         uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
00274         uint8_t ck[2];
00275         mavlink_status_t *status = mavlink_get_channel_status(chan);
00276         uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
00277         uint8_t signature_len = 0;
00278         uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
00279         bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
00280         bool signing =  (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
00281 
00282         if (mavlink1) {
00283             length = min_length;
00284             if (msgid > 255) {
00285                 // can't send 16 bit messages
00286                 _mav_parse_error(status);
00287                 return;
00288             }
00289             header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
00290             buf[0] = MAVLINK_STX_MAVLINK1;
00291             buf[1] = length;
00292             buf[2] = status->current_tx_seq;
00293             buf[3] = mavlink_system.sysid;
00294             buf[4] = mavlink_system.compid;
00295             buf[5] = msgid & 0xFF;
00296         } else {
00297             uint8_t incompat_flags = 0;
00298             if (signing) {
00299                 incompat_flags |= MAVLINK_IFLAG_SIGNED;
00300             }
00301             buf[0] = MAVLINK_STX;
00302             buf[1] = length;
00303             buf[2] = incompat_flags;
00304             buf[3] = 0; // compat_flags
00305             buf[4] = status->current_tx_seq;
00306             buf[5] = mavlink_system.sysid;
00307             buf[6] = mavlink_system.compid;
00308             buf[7] = msgid & 0xFF;
00309             buf[8] = (msgid >> 8) & 0xFF;
00310             buf[9] = (msgid >> 16) & 0xFF;
00311         }
00312         status->current_tx_seq++;
00313         checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
00314         crc_accumulate_buffer(&checksum, packet, length);
00315         crc_accumulate(crc_extra, &checksum);
00316         ck[0] = (uint8_t)(checksum & 0xFF);
00317         ck[1] = (uint8_t)(checksum >> 8);
00318 
00319         if (signing) {
00320                 // possibly add a signature
00321                 signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
00322                                                     (const uint8_t *)packet, length, ck);
00323         }
00324         
00325         MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
00326         _mavlink_send_uart(chan, (const char *)buf, header_len+1);
00327         _mavlink_send_uart(chan, packet, length);
00328         _mavlink_send_uart(chan, (const char *)ck, 2);
00329         if (signature_len != 0) {
00330                 _mavlink_send_uart(chan, (const char *)signature, signature_len);
00331         }
00332         MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
00333 }
00334 
00340 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
00341 {
00342         uint8_t ck[2];
00343 
00344         ck[0] = (uint8_t)(msg->checksum & 0xFF);
00345         ck[1] = (uint8_t)(msg->checksum >> 8);
00346         // XXX use the right sequence here
00347 
00348         uint8_t header_len = MAVLINK_CORE_HEADER_LEN + 1;
00349         if (msg->magic == MAVLINK_STX_MAVLINK1) {
00350             header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
00351         }
00352         MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2);
00353         _mavlink_send_uart(chan, (const char *)&msg->magic, header_len);
00354         _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
00355         _mavlink_send_uart(chan, (const char *)ck, 2);
00356         MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2);
00357 }
00358 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
00359 
00363 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
00364 {
00365         uint8_t signature_len, header_len;
00366         uint8_t *ck;
00367         
00368         if (msg->magic == MAVLINK_STX_MAVLINK1) {
00369                 signature_len = 0;
00370                 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
00371                 buf[0] = msg->magic;
00372                 buf[1] = msg->len;
00373                 buf[2] = msg->seq;
00374                 buf[3] = msg->sysid;
00375                 buf[4] = msg->compid;
00376                 buf[5] = msg->msgid & 0xFF;
00377                 memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
00378                 ck = buf + header_len + 1 + (uint16_t)msg->len;
00379         } else {
00380                 header_len = MAVLINK_CORE_HEADER_LEN;
00381                 buf[0] = msg->magic;
00382                 buf[1] = msg->len;
00383                 buf[2] = msg->incompat_flags;
00384                 buf[3] = msg->compat_flags;
00385                 buf[4] = msg->seq;
00386                 buf[5] = msg->sysid;
00387                 buf[6] = msg->compid;
00388                 buf[7] = msg->msgid & 0xFF;
00389                 buf[8] = (msg->msgid >> 8) & 0xFF;
00390                 buf[9] = (msg->msgid >> 16) & 0xFF;
00391                 memcpy(&buf[10], _MAV_PAYLOAD(msg), msg->len);
00392                 ck = buf + header_len + 1 + (uint16_t)msg->len;
00393                 signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
00394         }
00395         ck[0] = (uint8_t)(msg->checksum & 0xFF);
00396         ck[1] = (uint8_t)(msg->checksum >> 8);
00397         if (signature_len > 0) {
00398                 memcpy(&ck[2], msg->signature, signature_len);
00399         }
00400 
00401         return header_len + 1 + 2 + (uint16_t)msg->len + (uint16_t)signature_len;
00402 }
00403 
00404 union __mavlink_bitfield {
00405         uint8_t uint8;
00406         int8_t int8;
00407         uint16_t uint16;
00408         int16_t int16;
00409         uint32_t uint32;
00410         int32_t int32;
00411 };
00412 
00413 
00414 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
00415 {
00416         crc_init(&msg->checksum);
00417 }
00418 
00419 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
00420 {
00421         crc_accumulate(c, &msg->checksum);
00422 }
00423 
00424 /*
00425   return the crc_entry value for a msgid
00426 */
00427 MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
00428 {
00429         static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
00430         /*
00431           use a bisection search to find the right entry. A perfect hash may be better
00432           Note that this assumes the table is sorted by msgid
00433         */
00434         uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
00435         while (low < high) {
00436             uint32_t mid = (low+1+high)/2;
00437             if (msgid < mavlink_message_crcs[mid].msgid) {
00438                 high = mid-1;
00439                 continue;
00440             }
00441             if (msgid > mavlink_message_crcs[mid].msgid) {
00442                 low = mid;
00443                 continue;
00444             }
00445             low = mid;
00446             break;
00447         }
00448         if (mavlink_message_crcs[low].msgid != msgid) {
00449             // msgid is not in the table
00450             return NULL;
00451         }
00452         return &mavlink_message_crcs[low];
00453 }
00454 
00455 /*
00456   return the crc_extra value for a message
00457 */
00458 MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
00459 {
00460         const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
00461         return e?e->crc_extra:0;
00462 }
00463 
00464 /*
00465   return the expected message length
00466 */
00467 #define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
00468 MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg)
00469 {
00470         const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
00471         return e?e->msg_len:0;
00472 }
00473 
00508 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
00509                                                  mavlink_status_t* status,
00510                                                  uint8_t c, 
00511                                                  mavlink_message_t* r_message, 
00512                                                  mavlink_status_t* r_mavlink_status)
00513 {
00514         /* Enable this option to check the length of each message.
00515            This allows invalid messages to be caught much sooner. Use if the transmission
00516            medium is prone to missing (or extra) characters (e.g. a radio that fades in
00517            and out). Only use if the channel will only contain messages types listed in
00518            the headers.
00519         */
00520 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00521 #ifndef MAVLINK_MESSAGE_LENGTH
00522         static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
00523 #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
00524 #endif
00525 #endif
00526 
00527         int bufferIndex = 0;
00528 
00529         status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00530 
00531         switch (status->parse_state)
00532         {
00533         case MAVLINK_PARSE_STATE_UNINIT:
00534         case MAVLINK_PARSE_STATE_IDLE:
00535                 if (c == MAVLINK_STX)
00536                 {
00537                         status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00538                         rxmsg->len = 0;
00539                         rxmsg->magic = c;
00540                         status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
00541                         mavlink_start_checksum(rxmsg);
00542                 } else if (c == MAVLINK_STX_MAVLINK1)
00543                 {
00544                         status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00545                         rxmsg->len = 0;
00546                         rxmsg->magic = c;
00547                         status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
00548                         mavlink_start_checksum(rxmsg);
00549                 }
00550                 break;
00551 
00552         case MAVLINK_PARSE_STATE_GOT_STX:
00553                         if (status->msg_received 
00554 /* Support shorter buffers than the
00555    default maximum packet size */
00556 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
00557                                 || c > MAVLINK_MAX_PAYLOAD_LEN
00558 #endif
00559                                 )
00560                 {
00561                         status->buffer_overrun++;
00562                         _mav_parse_error(status);
00563                         status->msg_received = 0;
00564                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00565                 }
00566                 else
00567                 {
00568                         // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00569                         rxmsg->len = c;
00570                         status->packet_idx = 0;
00571                         mavlink_update_checksum(rxmsg, c);
00572                         if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
00573                             rxmsg->incompat_flags = 0;
00574                             rxmsg->compat_flags = 0;
00575                             status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
00576                         } else {
00577                             status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00578                         }
00579                 }
00580                 break;
00581 
00582         case MAVLINK_PARSE_STATE_GOT_LENGTH:
00583                 rxmsg->incompat_flags = c;
00584                 if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
00585                         // message includes an incompatible feature flag
00586                         _mav_parse_error(status);
00587                         status->msg_received = 0;
00588                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00589                         break;
00590                 }
00591                 mavlink_update_checksum(rxmsg, c);
00592                 status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
00593                 break;
00594 
00595         case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
00596                 rxmsg->compat_flags = c;
00597                 mavlink_update_checksum(rxmsg, c);
00598                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
00599                 break;
00600 
00601         case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
00602                 rxmsg->seq = c;
00603                 mavlink_update_checksum(rxmsg, c);
00604                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00605                 break;
00606                 
00607         case MAVLINK_PARSE_STATE_GOT_SEQ:
00608                 rxmsg->sysid = c;
00609                 mavlink_update_checksum(rxmsg, c);
00610                 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00611                 break;
00612 
00613         case MAVLINK_PARSE_STATE_GOT_SYSID:
00614                 rxmsg->compid = c;
00615                 mavlink_update_checksum(rxmsg, c);
00616                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00617                 break;
00618 
00619         case MAVLINK_PARSE_STATE_GOT_COMPID:
00620                 rxmsg->msgid = c;
00621                 mavlink_update_checksum(rxmsg, c);
00622                 if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
00623                     status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
00624 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00625                     if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
00626                     {
00627                         _mav_parse_error(status);
00628                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00629                         break;
00630                     }
00631 #endif
00632                 } else {
00633                     status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
00634                 }
00635                 break;
00636 
00637         case MAVLINK_PARSE_STATE_GOT_MSGID1:
00638                 rxmsg->msgid |= c<<8;
00639                 mavlink_update_checksum(rxmsg, c);
00640                 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
00641                 break;
00642 
00643         case MAVLINK_PARSE_STATE_GOT_MSGID2:
00644                 rxmsg->msgid |= c<<16;
00645                 mavlink_update_checksum(rxmsg, c);
00646                 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
00647 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00648                 if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
00649                 {
00650                         _mav_parse_error(status);
00651                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00652                         break;
00653                 }
00654 #endif
00655                 break;
00656                 
00657         case MAVLINK_PARSE_STATE_GOT_MSGID3:
00658                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
00659                 mavlink_update_checksum(rxmsg, c);
00660                 if (status->packet_idx == rxmsg->len)
00661                 {
00662                         status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00663                 }
00664                 break;
00665 
00666         case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
00667                 uint8_t crc_extra = mavlink_get_crc_extra(rxmsg);
00668                 mavlink_update_checksum(rxmsg, crc_extra);
00669                 if (c != (rxmsg->checksum & 0xFF)) {
00670                         status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
00671                 } else {
00672                         status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00673                 }
00674                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
00675                 break;
00676         }
00677 
00678         case MAVLINK_PARSE_STATE_GOT_CRC1:
00679         case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
00680                 if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
00681                         // got a bad CRC message
00682                         status->msg_received = MAVLINK_FRAMING_BAD_CRC;
00683                 } else {
00684                         // Successfully got message
00685                         status->msg_received = MAVLINK_FRAMING_OK;
00686                 }
00687                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
00688                 if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
00689                         status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
00690                         status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
00691                         status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00692                 } else {
00693                         if (status->signing &&
00694                             (status->signing->accept_unsigned_callback == NULL ||
00695                              !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
00696                                 // don't accept this unsigned packet
00697                                 status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
00698                         }
00699                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00700                         memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00701                 }
00702                 break;
00703         case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
00704                 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+2+(MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait)] = (char)c;
00705                 status->signature_wait--;
00706                 if (status->signature_wait == 0) {
00707                         // we have the whole signature, check it is OK
00708                         bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
00709                         if (!sig_ok &&
00710                             (status->signing->accept_unsigned_callback &&
00711                              status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
00712                                 // accepted via application level override
00713                                 sig_ok = true;
00714                         }
00715                         if (sig_ok) {
00716                                 status->msg_received = MAVLINK_FRAMING_OK;
00717                         } else {
00718                                 status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
00719                         }
00720                         status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00721                         memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
00722                 }
00723                 break;
00724         }
00725 
00726         bufferIndex++;
00727         // If a message has been sucessfully decoded, check index
00728         if (status->msg_received == MAVLINK_FRAMING_OK)
00729         {
00730                 //while(status->current_seq != rxmsg->seq)
00731                 //{
00732                 //      status->packet_rx_drop_count++;
00733                 //               status->current_seq++;
00734                 //}
00735                 status->current_rx_seq = rxmsg->seq;
00736                 // Initial condition: If no packet has been received so far, drop count is undefined
00737                 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00738                 // Count this packet as received
00739                 status->packet_rx_success_count++;
00740         }
00741 
00742         r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
00743         r_mavlink_status->parse_state = status->parse_state;
00744         r_mavlink_status->packet_idx = status->packet_idx;
00745         r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
00746         r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
00747         r_mavlink_status->packet_rx_drop_count = status->parse_error;
00748         r_mavlink_status->flags = status->flags;
00749         status->parse_error = 0;
00750 
00751         if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
00752                 /*
00753                   the CRC came out wrong. We now need to overwrite the
00754                   msg CRC with the one on the wire so that if the
00755                   caller decides to forward the message anyway that
00756                   mavlink_msg_to_send_buffer() won't overwrite the
00757                   checksum
00758                  */
00759                 r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
00760         }
00761 
00762         return status->msg_received;
00763 }
00764 
00806 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00807 {
00808         return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
00809                                          mavlink_get_channel_status(chan),
00810                                          c,
00811                                          r_message,
00812                                          r_mavlink_status);
00813 }
00814 
00815 
00856 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
00857 {
00858     uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
00859     if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
00860         msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
00861             // we got a bad CRC. Treat as a parse failure
00862             mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
00863             mavlink_status_t* status = mavlink_get_channel_status(chan);
00864             _mav_parse_error(status);
00865             status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
00866             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00867             if (c == MAVLINK_STX)
00868             {
00869                     status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00870                     rxmsg->len = 0;
00871                     mavlink_start_checksum(rxmsg);
00872             }
00873             return 0;
00874     }
00875     return msg_received;
00876 }
00877 
00888 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)
00889 {
00890         uint16_t bits_remain = bits;
00891         // Transform number into network order
00892         int32_t v;
00893         uint8_t i_bit_index, i_byte_index, curr_bits_n;
00894 #if MAVLINK_NEED_BYTE_SWAP
00895         union {
00896                 int32_t i;
00897                 uint8_t b[4];
00898         } bin, bout;
00899         bin.i = b;
00900         bout.b[0] = bin.b[3];
00901         bout.b[1] = bin.b[2];
00902         bout.b[2] = bin.b[1];
00903         bout.b[3] = bin.b[0];
00904         v = bout.i;
00905 #else
00906         v = b;
00907 #endif
00908 
00909         // buffer in
00910         // 01100000 01000000 00000000 11110001
00911         // buffer out
00912         // 11110001 00000000 01000000 01100000
00913 
00914         // Existing partly filled byte (four free slots)
00915         // 0111xxxx
00916 
00917         // Mask n free bits
00918         // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
00919         // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
00920 
00921         // Shift n bits into the right position
00922         // out = in >> n;
00923 
00924         // Mask and shift bytes
00925         i_bit_index = bit_index;
00926         i_byte_index = packet_index;
00927         if (bit_index > 0)
00928         {
00929                 // If bits were available at start, they were available
00930                 // in the byte before the current index
00931                 i_byte_index--;
00932         }
00933 
00934         // While bits have not been packed yet
00935         while (bits_remain > 0)
00936         {
00937                 // Bits still have to be packed
00938                 // there can be more than 8 bits, so
00939                 // we might have to pack them into more than one byte
00940 
00941                 // First pack everything we can into the current 'open' byte
00942                 //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
00943                 //FIXME
00944                 if (bits_remain <= (uint8_t)(8 - i_bit_index))
00945                 {
00946                         // Enough space
00947                         curr_bits_n = (uint8_t)bits_remain;
00948                 }
00949                 else
00950                 {
00951                         curr_bits_n = (8 - i_bit_index);
00952                 }
00953                 
00954                 // Pack these n bits into the current byte
00955                 // Mask out whatever was at that position with ones (xxx11111)
00956                 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
00957                 // Put content to this position, by masking out the non-used part
00958                 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
00959                 
00960                 // Increment the bit index
00961                 i_bit_index += curr_bits_n;
00962 
00963                 // Now proceed to the next byte, if necessary
00964                 bits_remain -= curr_bits_n;
00965                 if (bits_remain > 0)
00966                 {
00967                         // Offer another 8 bits / one byte
00968                         i_byte_index++;
00969                         i_bit_index = 0;
00970                 }
00971         }
00972         
00973         *r_bit_index = i_bit_index;
00974         // If a partly filled byte is present, mark this as consumed
00975         if (i_bit_index != 7) i_byte_index++;
00976         return i_byte_index - packet_index;
00977 }
00978 
00979 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00980 
00981 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
00982 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
00983 // whole packet at a time
00984 
00985 /*
00986 
00987 #include "mavlink_types.h"
00988 
00989 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
00990 {
00991     if (chan == MAVLINK_COMM_0)
00992     {
00993         uart0_transmit(ch);
00994     }
00995     if (chan == MAVLINK_COMM_1)
00996     {
00997         uart1_transmit(ch);
00998     }
00999 }
01000  */
01001 
01002 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
01003 {
01004 #ifdef MAVLINK_SEND_UART_BYTES
01005         /* this is the more efficient approach, if the platform
01006            defines it */
01007         MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
01008 #else
01009         /* fallback to one byte at a time */
01010         uint16_t i;
01011         for (i = 0; i < len; i++) {
01012                 comm_send_ch(chan, (uint8_t)buf[i]);
01013         }
01014 #endif
01015 }
01016 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
01017 
01018 #endif /* _MAVLINK_HELPERS_H_ */
01019 
01020 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43