source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/mavlink_generated_messages/mavlink_helpers.h @ 46

Last change on this file since 46 was 46, checked in by Thomas Fuhrmann, 5 years ago

Add the mavlink branch

File size: 34.7 KB
Line 
1#ifndef  _MAVLINK_HELPERS_H_
2#define  _MAVLINK_HELPERS_H_
3
4#include "string.h"
5#include "checksum.h"
6#include "mavlink_types.h"
7#include "mavlink_conversions.h"
8#include <stdio.h>
9
10#ifndef MAVLINK_HELPER
11#define MAVLINK_HELPER
12#endif
13
14#include "mavlink_sha256.h"
15
16/*
17 * Internal function to give access to the channel status for each channel
18 */
19#ifndef MAVLINK_GET_CHANNEL_STATUS
20MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
21{
22#ifdef MAVLINK_EXTERNAL_RX_STATUS
23        // No m_mavlink_status array defined in function,
24        // has to be defined externally
25#else
26        static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
27#endif
28        return &m_mavlink_status[chan];
29}
30#endif
31
32/*
33 * Internal function to give access to the channel buffer for each channel
34 */
35#ifndef MAVLINK_GET_CHANNEL_BUFFER
36MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
37{
38       
39#ifdef MAVLINK_EXTERNAL_RX_BUFFER
40        // No m_mavlink_buffer array defined in function,
41        // has to be defined externally
42#else
43        static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
44#endif
45        return &m_mavlink_buffer[chan];
46}
47#endif // MAVLINK_GET_CHANNEL_BUFFER
48
49/**
50 * @brief Reset the status of a channel.
51 */
52MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
53{
54        mavlink_status_t *status = mavlink_get_channel_status(chan);
55        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
56}
57
58/**
59 * @brief create a signature block for a packet
60 */
61MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
62                                           uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
63                                           const uint8_t *header, uint8_t header_len,
64                                           const uint8_t *packet, uint8_t packet_len,
65                                           const uint8_t crc[2])
66{
67        mavlink_sha256_ctx ctx;
68        union {
69            uint64_t t64;
70            uint8_t t8[8];
71        } tstamp;
72        if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
73            return 0;
74        }
75        signature[0] = signing->link_id;
76        tstamp.t64 = signing->timestamp;
77        memcpy(&signature[1], tstamp.t8, 6);
78        signing->timestamp++;
79       
80        mavlink_sha256_init(&ctx);
81        mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
82        mavlink_sha256_update(&ctx, header, header_len);
83        mavlink_sha256_update(&ctx, packet, packet_len);
84        mavlink_sha256_update(&ctx, crc, 2);
85        mavlink_sha256_update(&ctx, signature, 7);
86        mavlink_sha256_final_48(&ctx, &signature[7]);
87       
88        return MAVLINK_SIGNATURE_BLOCK_LEN;
89}
90
91/**
92 * return new packet length for trimming payload of any trailing zero
93 * bytes. Used in MAVLink2 to give simple support for variable length
94 * arrays.
95 */
96MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
97{
98        while (length > 0 && payload[length-1] == 0) {
99                length--;
100        }
101        return length;
102}
103
104/**
105 * @brief check a signature block for a packet
106 */
107MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
108                                            mavlink_signing_streams_t *signing_streams,
109                                            const mavlink_message_t *msg)
110{
111        if (signing == NULL) {
112                return true;
113        }
114        const uint8_t *p = (const uint8_t *)&msg->magic;
115        const uint8_t *psig = msg->signature;
116        const uint8_t *incoming_signature = psig+7;
117        mavlink_sha256_ctx ctx;
118        uint8_t signature[6];
119        uint16_t i;
120       
121        mavlink_sha256_init(&ctx);
122        mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
123        mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len);
124        mavlink_sha256_update(&ctx, msg->ck, 2);
125        mavlink_sha256_update(&ctx, psig, 1+6);
126        mavlink_sha256_final_48(&ctx, signature);
127        if (memcmp(signature, incoming_signature, 6) != 0) {
128                return false;
129        }
130
131        // now check timestamp
132        union tstamp {
133            uint64_t t64;
134            uint8_t t8[8];
135        } tstamp;
136        uint8_t link_id = psig[0];
137        tstamp.t64 = 0;
138        memcpy(tstamp.t8, psig+1, 6);
139
140        if (signing_streams == NULL) {
141                return false;
142        }
143       
144        // find stream
145        for (i=0; i<signing_streams->num_signing_streams; i++) {
146                if (msg->sysid == signing_streams->stream[i].sysid &&
147                    msg->compid == signing_streams->stream[i].compid &&
148                    link_id == signing_streams->stream[i].link_id) {
149                        break;
150                }
151        }
152        if (i == signing_streams->num_signing_streams) {
153                if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
154                        // over max number of streams
155                        return false;
156                }
157                // new stream. Only accept if timestamp is not more than 1 minute old
158                if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
159                        return false;
160                }
161                // add new stream
162                signing_streams->stream[i].sysid = msg->sysid;
163                signing_streams->stream[i].compid = msg->compid;
164                signing_streams->stream[i].link_id = link_id;
165                signing_streams->num_signing_streams++;
166        } else {
167                union tstamp last_tstamp;
168                last_tstamp.t64 = 0;
169                memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
170                if (tstamp.t64 <= last_tstamp.t64) {
171                        // repeating old timestamp
172                        return false;
173                }
174        }
175
176        // remember last timestamp
177        memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
178
179        // our next timestamp must be at least this timestamp
180        if (tstamp.t64 > signing->timestamp) {
181                signing->timestamp = tstamp.t64;
182        }
183        return true;
184}
185
186
187/**
188 * @brief Finalize a MAVLink message with channel assignment
189 *
190 * This function calculates the checksum and sets length and aircraft id correctly.
191 * It assumes that the message id and the payload are already correctly set. This function
192 * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
193 * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
194 *
195 * @param msg Message to finalize
196 * @param system_id Id of the sending (this) system, 1-127
197 * @param length Message length
198 */
199MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
200                                                      uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
201{
202        mavlink_status_t *status = mavlink_get_channel_status(chan);
203        bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
204        bool signing =  (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
205        uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
206        uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
207        uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
208        if (mavlink1) {
209                msg->magic = MAVLINK_STX_MAVLINK1;
210                header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
211        } else {
212                msg->magic = MAVLINK_STX;
213        }
214        msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
215        msg->sysid = system_id;
216        msg->compid = component_id;
217        msg->incompat_flags = 0;
218        if (signing) {
219                msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
220        }
221        msg->compat_flags = 0;
222        msg->seq = status->current_tx_seq;
223        status->current_tx_seq = status->current_tx_seq + 1;
224
225        // form the header as a byte array for the crc
226        buf[0] = msg->magic;
227        buf[1] = msg->len;
228        if (mavlink1) {
229                buf[2] = msg->seq;
230                buf[3] = msg->sysid;
231                buf[4] = msg->compid;
232                buf[5] = msg->msgid & 0xFF;
233        } else {
234                buf[2] = msg->incompat_flags;
235                buf[3] = msg->compat_flags;
236                buf[4] = msg->seq;
237                buf[5] = msg->sysid;
238                buf[6] = msg->compid;
239                buf[7] = msg->msgid & 0xFF;
240                buf[8] = (msg->msgid >> 8) & 0xFF;
241                buf[9] = (msg->msgid >> 16) & 0xFF;
242        }
243       
244        msg->checksum = crc_calculate(&buf[1], header_len-1);
245        crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
246        crc_accumulate(crc_extra, &msg->checksum);
247        mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
248        mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
249
250        if (signing) {
251                mavlink_sign_packet(status->signing,
252                                    msg->signature,
253                                    (const uint8_t *)buf, header_len,
254                                    (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
255                                    (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
256        }
257       
258        return msg->len + header_len + 2 + signature_len;
259}
260
261
262/**
263 * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
264 */
265MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
266                                                 uint8_t min_length, uint8_t length, uint8_t crc_extra)
267{
268    return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
269}
270
271static inline void _mav_parse_error(mavlink_status_t *status)
272{
273    status->parse_error++;
274}
275
276#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
277MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
278
279/**
280 * @brief Finalize a MAVLink message with channel assignment and send
281 */
282MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
283                                                    const char *packet, 
284                                                    uint8_t min_length, uint8_t length, uint8_t crc_extra)
285{
286        uint16_t checksum;
287        uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
288        uint8_t ck[2];
289        mavlink_status_t *status = mavlink_get_channel_status(chan);
290        uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
291        uint8_t signature_len = 0;
292        uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
293        bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
294        bool signing =  (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
295
296        if (mavlink1) {
297            length = min_length;
298            if (msgid > 255) {
299                // can't send 16 bit messages
300                _mav_parse_error(status);
301                return;
302            }
303            header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
304            buf[0] = MAVLINK_STX_MAVLINK1;
305            buf[1] = length;
306            buf[2] = status->current_tx_seq;
307            buf[3] = mavlink_system.sysid;
308            buf[4] = mavlink_system.compid;
309            buf[5] = msgid & 0xFF;
310        } else {
311            uint8_t incompat_flags = 0;
312            if (signing) {
313                incompat_flags |= MAVLINK_IFLAG_SIGNED;
314            }
315            length = _mav_trim_payload(packet, length);
316            buf[0] = MAVLINK_STX;
317            buf[1] = length;
318            buf[2] = incompat_flags;
319            buf[3] = 0; // compat_flags
320            buf[4] = status->current_tx_seq;
321            buf[5] = mavlink_system.sysid;
322            buf[6] = mavlink_system.compid;
323            buf[7] = msgid & 0xFF;
324            buf[8] = (msgid >> 8) & 0xFF;
325            buf[9] = (msgid >> 16) & 0xFF;
326        }
327        status->current_tx_seq++;
328        checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
329        crc_accumulate_buffer(&checksum, packet, length);
330        crc_accumulate(crc_extra, &checksum);
331        ck[0] = (uint8_t)(checksum & 0xFF);
332        ck[1] = (uint8_t)(checksum >> 8);
333
334        if (signing) {
335                // possibly add a signature
336                signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
337                                                    (const uint8_t *)packet, length, ck);
338        }
339       
340        MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
341        _mavlink_send_uart(chan, (const char *)buf, header_len+1);
342        _mavlink_send_uart(chan, packet, length);
343        _mavlink_send_uart(chan, (const char *)ck, 2);
344        if (signature_len != 0) {
345                _mavlink_send_uart(chan, (const char *)signature, signature_len);
346        }
347        MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
348}
349
350/**
351 * @brief re-send a message over a uart channel
352 * this is more stack efficient than re-marshalling the message
353 * If the message is signed then the original signature is also sent
354 */
355MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
356{
357        uint8_t ck[2];
358
359        ck[0] = (uint8_t)(msg->checksum & 0xFF);
360        ck[1] = (uint8_t)(msg->checksum >> 8);
361        // XXX use the right sequence here
362
363        uint8_t header_len;
364        uint8_t signature_len;
365       
366        if (msg->magic == MAVLINK_STX_MAVLINK1) {
367            header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
368            signature_len = 0;
369            MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
370            // we can't send the structure directly as it has extra mavlink2 elements in it
371            uint8_t buf[header_len];
372            buf[0] = msg->magic;
373            buf[1] = msg->len;
374            buf[2] = msg->seq;
375            buf[3] = msg->sysid;
376            buf[4] = msg->compid;
377            buf[5] = msg->msgid & 0xFF;
378            _mavlink_send_uart(chan, (const char*)buf, header_len);
379        } else {
380            header_len = MAVLINK_CORE_HEADER_LEN + 1;
381            signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
382            MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
383            uint8_t buf[header_len];
384            buf[0] = msg->magic;
385            buf[1] = msg->len;
386            buf[2] = msg->incompat_flags;
387            buf[3] = msg->compat_flags;
388            buf[4] = msg->seq;
389            buf[5] = msg->sysid;
390            buf[6] = msg->compid;
391            buf[7] = msg->msgid & 0xFF;
392            buf[8] = (msg->msgid >> 8) & 0xFF;
393            buf[9] = (msg->msgid >> 16) & 0xFF;
394            _mavlink_send_uart(chan, (const char *)buf, header_len);
395        }
396        _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
397        _mavlink_send_uart(chan, (const char *)ck, 2);
398        if (signature_len != 0) {
399            _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
400        }
401        MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
402}
403#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
404
405/**
406 * @brief Pack a message to send it over a serial byte stream
407 */
408MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
409{
410        uint8_t signature_len, header_len;
411        uint8_t *ck;
412        uint8_t length = msg->len;
413       
414        if (msg->magic == MAVLINK_STX_MAVLINK1) {
415                signature_len = 0;
416                header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
417                buf[0] = msg->magic;
418                buf[1] = length;
419                buf[2] = msg->seq;
420                buf[3] = msg->sysid;
421                buf[4] = msg->compid;
422                buf[5] = msg->msgid & 0xFF;
423                memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
424                ck = buf + header_len + 1 + (uint16_t)msg->len;
425        } else {
426                length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
427                header_len = MAVLINK_CORE_HEADER_LEN;
428                buf[0] = msg->magic;
429                buf[1] = length;
430                buf[2] = msg->incompat_flags;
431                buf[3] = msg->compat_flags;
432                buf[4] = msg->seq;
433                buf[5] = msg->sysid;
434                buf[6] = msg->compid;
435                buf[7] = msg->msgid & 0xFF;
436                buf[8] = (msg->msgid >> 8) & 0xFF;
437                buf[9] = (msg->msgid >> 16) & 0xFF;
438                memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
439                ck = buf + header_len + 1 + (uint16_t)length;
440                signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
441        }
442        ck[0] = (uint8_t)(msg->checksum & 0xFF);
443        ck[1] = (uint8_t)(msg->checksum >> 8);
444        if (signature_len > 0) {
445                memcpy(&ck[2], msg->signature, signature_len);
446        }
447
448        return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
449}
450
451union __mavlink_bitfield {
452        uint8_t uint8;
453        int8_t int8;
454        uint16_t uint16;
455        int16_t int16;
456        uint32_t uint32;
457        int32_t int32;
458};
459
460
461MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
462{
463        crc_init(&msg->checksum);
464}
465
466MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
467{
468        crc_accumulate(c, &msg->checksum);
469}
470
471/*
472  return the crc_entry value for a msgid
473*/
474MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
475{
476        static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
477        /*
478          use a bisection search to find the right entry. A perfect hash may be better
479          Note that this assumes the table is sorted by msgid
480        */
481        uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
482        while (low < high) {
483            uint32_t mid = (low+1+high)/2;
484            if (msgid < mavlink_message_crcs[mid].msgid) {
485                high = mid-1;
486                continue;
487            }
488            if (msgid > mavlink_message_crcs[mid].msgid) {
489                low = mid;
490                continue;
491            }
492            low = mid;
493            break;
494        }
495        if (mavlink_message_crcs[low].msgid != msgid) {
496            // msgid is not in the table
497            return NULL;
498        }
499        return &mavlink_message_crcs[low];
500}
501
502/*
503  return the crc_extra value for a message
504*/
505MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
506{
507        const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
508        return e?e->crc_extra:0;
509}
510
511/*
512  return the expected message length
513*/
514#define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
515MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg)
516{
517        const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
518        return e?e->msg_len:0;
519}
520
521/**
522 * This is a varient of mavlink_frame_char() but with caller supplied
523 * parsing buffers. It is useful when you want to create a MAVLink
524 * parser in a library that doesn't use any global variables
525 *
526 * @param rxmsg    parsing message buffer
527 * @param status   parsing starus buffer
528 * @param c        The char to parse
529 *
530 * @param returnMsg NULL if no message could be decoded, the message data else
531 * @param returnStats if a message was decoded, this is filled with the channel's stats
532 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
533 *
534 * A typical use scenario of this function call is:
535 *
536 * @code
537 * #include <mavlink.h>
538 *
539 * mavlink_message_t msg;
540 * int chan = 0;
541 *
542 *
543 * while(serial.bytesAvailable > 0)
544 * {
545 *   uint8_t byte = serial.getNextByte();
546 *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
547 *     {
548 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
549 *     }
550 * }
551 *
552 *
553 * @endcode
554 */
555MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
556                                                 mavlink_status_t* status,
557                                                 uint8_t c, 
558                                                 mavlink_message_t* r_message, 
559                                                 mavlink_status_t* r_mavlink_status)
560{
561        /* Enable this option to check the length of each message.
562           This allows invalid messages to be caught much sooner. Use if the transmission
563           medium is prone to missing (or extra) characters (e.g. a radio that fades in
564           and out). Only use if the channel will only contain messages types listed in
565           the headers.
566        */
567#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
568#ifndef MAVLINK_MESSAGE_LENGTH
569        static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
570#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
571#endif
572#endif
573
574        int bufferIndex = 0;
575
576        status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
577
578        switch (status->parse_state)
579        {
580        case MAVLINK_PARSE_STATE_UNINIT:
581        case MAVLINK_PARSE_STATE_IDLE:
582                if (c == MAVLINK_STX)
583                {
584                        status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
585                        rxmsg->len = 0;
586                        rxmsg->magic = c;
587                        status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
588                        mavlink_start_checksum(rxmsg);
589                } else if (c == MAVLINK_STX_MAVLINK1)
590                {
591                        status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
592                        rxmsg->len = 0;
593                        rxmsg->magic = c;
594                        status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
595                        mavlink_start_checksum(rxmsg);
596                }
597                break;
598
599        case MAVLINK_PARSE_STATE_GOT_STX:
600                        if (status->msg_received
601/* Support shorter buffers than the
602   default maximum packet size */
603#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
604                                || c > MAVLINK_MAX_PAYLOAD_LEN
605#endif
606                                )
607                {
608                        status->buffer_overrun++;
609                        _mav_parse_error(status);
610                        status->msg_received = 0;
611                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
612                }
613                else
614                {
615                        // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
616                        rxmsg->len = c;
617                        status->packet_idx = 0;
618                        mavlink_update_checksum(rxmsg, c);
619                        if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
620                            rxmsg->incompat_flags = 0;
621                            rxmsg->compat_flags = 0;
622                            status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
623                        } else {
624                            status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
625                        }
626                }
627                break;
628
629        case MAVLINK_PARSE_STATE_GOT_LENGTH:
630                rxmsg->incompat_flags = c;
631                if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
632                        // message includes an incompatible feature flag
633                        _mav_parse_error(status);
634                        status->msg_received = 0;
635                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
636                        break;
637                }
638                mavlink_update_checksum(rxmsg, c);
639                status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
640                break;
641
642        case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
643                rxmsg->compat_flags = c;
644                mavlink_update_checksum(rxmsg, c);
645                status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
646                break;
647
648        case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
649                rxmsg->seq = c;
650                mavlink_update_checksum(rxmsg, c);
651                status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
652                break;
653               
654        case MAVLINK_PARSE_STATE_GOT_SEQ:
655                rxmsg->sysid = c;
656                mavlink_update_checksum(rxmsg, c);
657                status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
658                break;
659
660        case MAVLINK_PARSE_STATE_GOT_SYSID:
661                rxmsg->compid = c;
662                mavlink_update_checksum(rxmsg, c);
663                status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
664                break;
665
666        case MAVLINK_PARSE_STATE_GOT_COMPID:
667                rxmsg->msgid = c;
668                mavlink_update_checksum(rxmsg, c);
669                if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
670                    status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
671#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
672                    if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
673                    {
674                        _mav_parse_error(status);
675                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
676                        break;
677                    }
678#endif
679                } else {
680                    status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
681                }
682                break;
683
684        case MAVLINK_PARSE_STATE_GOT_MSGID1:
685                rxmsg->msgid |= c<<8;
686                mavlink_update_checksum(rxmsg, c);
687                status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
688                break;
689
690        case MAVLINK_PARSE_STATE_GOT_MSGID2:
691                rxmsg->msgid |= c<<16;
692                mavlink_update_checksum(rxmsg, c);
693                status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
694#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
695                if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
696                {
697                        _mav_parse_error(status);
698                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
699                        break;
700                }
701#endif
702                break;
703               
704        case MAVLINK_PARSE_STATE_GOT_MSGID3:
705                _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
706                mavlink_update_checksum(rxmsg, c);
707                if (status->packet_idx == rxmsg->len)
708                {
709                        status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
710                }
711                break;
712
713        case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
714                const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
715                uint8_t crc_extra = e?e->crc_extra:0;
716                mavlink_update_checksum(rxmsg, crc_extra);
717                if (c != (rxmsg->checksum & 0xFF)) {
718                        status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
719                } else {
720                        status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
721                }
722                rxmsg->ck[0] = c;
723
724                // zero-fill the packet to cope with short incoming packets
725                if (e && status->packet_idx < e->msg_len) {
726                        memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx);
727                }
728                break;
729        }
730
731        case MAVLINK_PARSE_STATE_GOT_CRC1:
732        case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
733                if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
734                        // got a bad CRC message
735                        status->msg_received = MAVLINK_FRAMING_BAD_CRC;
736                } else {
737                        // Successfully got message
738                        status->msg_received = MAVLINK_FRAMING_OK;
739                }
740                rxmsg->ck[1] = c;
741                if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
742                        status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
743                        status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
744                        status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
745                } else {
746                        if (status->signing &&
747                            (status->signing->accept_unsigned_callback == NULL ||
748                             !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
749                                // don't accept this unsigned packet
750                                status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
751                        }
752                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
753                        memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
754                }
755                break;
756        case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
757                rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
758                status->signature_wait--;
759                if (status->signature_wait == 0) {
760                        // we have the whole signature, check it is OK
761                        bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
762                        if (!sig_ok &&
763                            (status->signing->accept_unsigned_callback &&
764                             status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
765                                // accepted via application level override
766                                sig_ok = true;
767                        }
768                        if (sig_ok) {
769                                status->msg_received = MAVLINK_FRAMING_OK;
770                        } else {
771                                status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
772                        }
773                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
774                        memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
775                }
776                break;
777        }
778
779        bufferIndex++;
780        // If a message has been sucessfully decoded, check index
781        if (status->msg_received == MAVLINK_FRAMING_OK)
782        {
783                //while(status->current_seq != rxmsg->seq)
784                //{
785                //      status->packet_rx_drop_count++;
786                //               status->current_seq++;
787                //}
788                status->current_rx_seq = rxmsg->seq;
789                // Initial condition: If no packet has been received so far, drop count is undefined
790                if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
791                // Count this packet as received
792                status->packet_rx_success_count++;
793        }
794
795        r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
796        r_mavlink_status->parse_state = status->parse_state;
797        r_mavlink_status->packet_idx = status->packet_idx;
798        r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
799        r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
800        r_mavlink_status->packet_rx_drop_count = status->parse_error;
801        r_mavlink_status->flags = status->flags;
802        status->parse_error = 0;
803
804        if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
805                /*
806                  the CRC came out wrong. We now need to overwrite the
807                  msg CRC with the one on the wire so that if the
808                  caller decides to forward the message anyway that
809                  mavlink_msg_to_send_buffer() won't overwrite the
810                  checksum
811                 */
812                r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
813        }
814
815        return status->msg_received;
816}
817
818/**
819 * This is a convenience function which handles the complete MAVLink parsing.
820 * the function will parse one byte at a time and return the complete packet once
821 * it could be successfully decoded. This function will return 0, 1 or
822 * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
823 *
824 * Messages are parsed into an internal buffer (one for each channel). When a complete
825 * message is received it is copies into *returnMsg and the channel's status is
826 * copied into *returnStats.
827 *
828 * @param chan     ID of the current channel. This allows to parse different channels with this function.
829 *                 a channel is not a physical message channel like a serial port, but a logic partition of
830 *                 the communication streams in this case. COMM_NB is the limit for the number of channels
831 *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
832 * @param c        The char to parse
833 *
834 * @param returnMsg NULL if no message could be decoded, the message data else
835 * @param returnStats if a message was decoded, this is filled with the channel's stats
836 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
837 *
838 * A typical use scenario of this function call is:
839 *
840 * @code
841 * #include <mavlink.h>
842 *
843 * mavlink_message_t msg;
844 * int chan = 0;
845 *
846 *
847 * while(serial.bytesAvailable > 0)
848 * {
849 *   uint8_t byte = serial.getNextByte();
850 *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
851 *     {
852 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
853 *     }
854 * }
855 *
856 *
857 * @endcode
858 */
859MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
860{
861        return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
862                                         mavlink_get_channel_status(chan),
863                                         c,
864                                         r_message,
865                                         r_mavlink_status);
866}
867
868/**
869 * Set the protocol version
870 */
871MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version)
872{
873        mavlink_status_t *status = mavlink_get_channel_status(chan);
874        if (version > 1) {
875                status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
876        } else {
877                status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
878        }
879}
880
881/**
882 * Get the protocol version
883 *
884 * @return 1 for v1, 2 for v2
885 */
886MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan, unsigned int version)
887{
888        mavlink_status_t *status = mavlink_get_channel_status(chan);
889        if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) {
890                return 1;
891        } else {
892                return 2;
893        }
894}
895
896/**
897 * This is a convenience function which handles the complete MAVLink parsing.
898 * the function will parse one byte at a time and return the complete packet once
899 * it could be successfully decoded. This function will return 0 or 1.
900 *
901 * Messages are parsed into an internal buffer (one for each channel). When a complete
902 * message is received it is copies into *returnMsg and the channel's status is
903 * copied into *returnStats.
904 *
905 * @param chan     ID of the current channel. This allows to parse different channels with this function.
906 *                 a channel is not a physical message channel like a serial port, but a logic partition of
907 *                 the communication streams in this case. COMM_NB is the limit for the number of channels
908 *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
909 * @param c        The char to parse
910 *
911 * @param returnMsg NULL if no message could be decoded, the message data else
912 * @param returnStats if a message was decoded, this is filled with the channel's stats
913 * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
914 *
915 * A typical use scenario of this function call is:
916 *
917 * @code
918 * #include <mavlink.h>
919 *
920 * mavlink_message_t msg;
921 * int chan = 0;
922 *
923 *
924 * while(serial.bytesAvailable > 0)
925 * {
926 *   uint8_t byte = serial.getNextByte();
927 *   if (mavlink_parse_char(chan, byte, &msg))
928 *     {
929 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
930 *     }
931 * }
932 *
933 *
934 * @endcode
935 */
936MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
937{
938    uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
939    if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
940        msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
941            // we got a bad CRC. Treat as a parse failure
942            mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
943            mavlink_status_t* status = mavlink_get_channel_status(chan);
944            _mav_parse_error(status);
945            status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
946            status->parse_state = MAVLINK_PARSE_STATE_IDLE;
947            if (c == MAVLINK_STX)
948            {
949                    status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
950                    rxmsg->len = 0;
951                    mavlink_start_checksum(rxmsg);
952            }
953            return 0;
954    }
955    return msg_received;
956}
957
958/**
959 * @brief Put a bitfield of length 1-32 bit into the buffer
960 *
961 * @param b the value to add, will be encoded in the bitfield
962 * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
963 * @param packet_index the position in the packet (the index of the first byte to use)
964 * @param bit_index the position in the byte (the index of the first bit to use)
965 * @param buffer packet buffer to write into
966 * @return new position of the last used byte in the buffer
967 */
968MAVLINK_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)
969{
970        uint16_t bits_remain = bits;
971        // Transform number into network order
972        int32_t v;
973        uint8_t i_bit_index, i_byte_index, curr_bits_n;
974#if MAVLINK_NEED_BYTE_SWAP
975        union {
976                int32_t i;
977                uint8_t b[4];
978        } bin, bout;
979        bin.i = b;
980        bout.b[0] = bin.b[3];
981        bout.b[1] = bin.b[2];
982        bout.b[2] = bin.b[1];
983        bout.b[3] = bin.b[0];
984        v = bout.i;
985#else
986        v = b;
987#endif
988
989        // buffer in
990        // 01100000 01000000 00000000 11110001
991        // buffer out
992        // 11110001 00000000 01000000 01100000
993
994        // Existing partly filled byte (four free slots)
995        // 0111xxxx
996
997        // Mask n free bits
998        // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
999        // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
1000
1001        // Shift n bits into the right position
1002        // out = in >> n;
1003
1004        // Mask and shift bytes
1005        i_bit_index = bit_index;
1006        i_byte_index = packet_index;
1007        if (bit_index > 0)
1008        {
1009                // If bits were available at start, they were available
1010                // in the byte before the current index
1011                i_byte_index--;
1012        }
1013
1014        // While bits have not been packed yet
1015        while (bits_remain > 0)
1016        {
1017                // Bits still have to be packed
1018                // there can be more than 8 bits, so
1019                // we might have to pack them into more than one byte
1020
1021                // First pack everything we can into the current 'open' byte
1022                //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
1023                //FIXME
1024                if (bits_remain <= (uint8_t)(8 - i_bit_index))
1025                {
1026                        // Enough space
1027                        curr_bits_n = (uint8_t)bits_remain;
1028                }
1029                else
1030                {
1031                        curr_bits_n = (8 - i_bit_index);
1032                }
1033               
1034                // Pack these n bits into the current byte
1035                // Mask out whatever was at that position with ones (xxx11111)
1036                buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
1037                // Put content to this position, by masking out the non-used part
1038                buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
1039               
1040                // Increment the bit index
1041                i_bit_index += curr_bits_n;
1042
1043                // Now proceed to the next byte, if necessary
1044                bits_remain -= curr_bits_n;
1045                if (bits_remain > 0)
1046                {
1047                        // Offer another 8 bits / one byte
1048                        i_byte_index++;
1049                        i_bit_index = 0;
1050                }
1051        }
1052       
1053        *r_bit_index = i_bit_index;
1054        // If a partly filled byte is present, mark this as consumed
1055        if (i_bit_index != 7) i_byte_index++;
1056        return i_byte_index - packet_index;
1057}
1058
1059#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
1060
1061// To make MAVLink work on your MCU, define comm_send_ch() if you wish
1062// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
1063// whole packet at a time
1064
1065/*
1066
1067#include "mavlink_types.h"
1068
1069void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
1070{
1071    if (chan == MAVLINK_COMM_0)
1072    {
1073        uart0_transmit(ch);
1074    }
1075    if (chan == MAVLINK_COMM_1)
1076    {
1077        uart1_transmit(ch);
1078    }
1079}
1080 */
1081
1082MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
1083{
1084#ifdef MAVLINK_SEND_UART_BYTES
1085        /* this is the more efficient approach, if the platform
1086           defines it */
1087        MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
1088#else
1089        /* fallback to one byte at a time */
1090        uint16_t i;
1091        for (i = 0; i < len; i++) {
1092                comm_send_ch(chan, (uint8_t)buf[i]);
1093        }
1094#endif
1095}
1096#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
1097
1098#endif /* _MAVLINK_HELPERS_H_ */
1099
1100
Note: See TracBrowser for help on using the repository browser.