source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/include/mavlink_helpers.h @ 77

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

Change the version of mavlink generated messages and rename it to include

File size: 21.2 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
9#ifndef MAVLINK_HELPER
10#define MAVLINK_HELPER
11#endif
12
13/*
14 * Internal function to give access to the channel status for each channel
15 */
16#ifndef MAVLINK_GET_CHANNEL_STATUS
17MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
18{
19#ifdef MAVLINK_EXTERNAL_RX_STATUS
20        // No m_mavlink_status array defined in function,
21        // has to be defined externally
22#else
23        static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
24#endif
25        return &m_mavlink_status[chan];
26}
27#endif
28
29/*
30 * Internal function to give access to the channel buffer for each channel
31 */
32#ifndef MAVLINK_GET_CHANNEL_BUFFER
33MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
34{
35       
36#ifdef MAVLINK_EXTERNAL_RX_BUFFER
37        // No m_mavlink_buffer array defined in function,
38        // has to be defined externally
39#else
40        static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
41#endif
42        return &m_mavlink_buffer[chan];
43}
44#endif
45
46/**
47 * @brief Reset the status of a channel.
48 */
49MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
50{
51        mavlink_status_t *status = mavlink_get_channel_status(chan);
52        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
53}
54
55/**
56 * @brief Finalize a MAVLink message with channel assignment
57 *
58 * This function calculates the checksum and sets length and aircraft id correctly.
59 * It assumes that the message id and the payload are already correctly set. This function
60 * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
61 * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
62 *
63 * @param msg Message to finalize
64 * @param system_id Id of the sending (this) system, 1-127
65 * @param length Message length
66 */
67#if MAVLINK_CRC_EXTRA
68MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
69                                                      uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
70#else
71MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
72                                                      uint8_t chan, uint8_t length)
73#endif
74{
75        // This code part is the same for all messages;
76        msg->magic = MAVLINK_STX;
77        msg->len = length;
78        msg->sysid = system_id;
79        msg->compid = component_id;
80        // One sequence number per channel
81        msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
82        mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
83        msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
84        crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
85#if MAVLINK_CRC_EXTRA
86        crc_accumulate(crc_extra, &msg->checksum);
87#endif
88        mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
89        mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
90
91        return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
92}
93
94
95/**
96 * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
97 */
98#if MAVLINK_CRC_EXTRA
99MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
100                                                 uint8_t min_length, uint8_t length, uint8_t crc_extra)
101{
102    return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
103}
104#else
105MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
106                                                 uint8_t length)
107{
108        return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
109}
110#endif
111
112#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
113MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
114
115/**
116 * @brief Finalize a MAVLink message with channel assignment and send
117 */
118#if MAVLINK_CRC_EXTRA
119MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
120                                                    uint8_t min_length, uint8_t length, uint8_t crc_extra)
121#else
122MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
123#endif
124{
125        uint16_t checksum;
126        uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
127        uint8_t ck[2];
128        mavlink_status_t *status = mavlink_get_channel_status(chan);
129        buf[0] = MAVLINK_STX;
130        buf[1] = length;
131        buf[2] = status->current_tx_seq;
132        buf[3] = mavlink_system.sysid;
133        buf[4] = mavlink_system.compid;
134        buf[5] = msgid;
135        status->current_tx_seq++;
136        checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
137        crc_accumulate_buffer(&checksum, packet, length);
138#if MAVLINK_CRC_EXTRA
139        crc_accumulate(crc_extra, &checksum);
140#endif
141        ck[0] = (uint8_t)(checksum & 0xFF);
142        ck[1] = (uint8_t)(checksum >> 8);
143
144        MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
145        _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
146        _mavlink_send_uart(chan, packet, length);
147        _mavlink_send_uart(chan, (const char *)ck, 2);
148        MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
149}
150
151/**
152 * @brief re-send a message over a uart channel
153 * this is more stack efficient than re-marshalling the message
154 */
155MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
156{
157        uint8_t ck[2];
158
159        ck[0] = (uint8_t)(msg->checksum & 0xFF);
160        ck[1] = (uint8_t)(msg->checksum >> 8);
161        // XXX use the right sequence here
162
163        MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
164        _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
165        _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
166        _mavlink_send_uart(chan, (const char *)ck, 2);
167        MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
168}
169#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
170
171/**
172 * @brief Pack a message to send it over a serial byte stream
173 */
174MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
175{
176        memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
177
178        uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
179
180        ck[0] = (uint8_t)(msg->checksum & 0xFF);
181        ck[1] = (uint8_t)(msg->checksum >> 8);
182
183        return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
184}
185
186union __mavlink_bitfield {
187        uint8_t uint8;
188        int8_t int8;
189        uint16_t uint16;
190        int16_t int16;
191        uint32_t uint32;
192        int32_t int32;
193};
194
195
196MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
197{
198        crc_init(&msg->checksum);
199}
200
201MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
202{
203        crc_accumulate(c, &msg->checksum);
204}
205
206/**
207 * This is a varient of mavlink_frame_char() but with caller supplied
208 * parsing buffers. It is useful when you want to create a MAVLink
209 * parser in a library that doesn't use any global variables
210 *
211 * @param rxmsg    parsing message buffer
212 * @param status   parsing starus buffer
213 * @param c        The char to parse
214 *
215 * @param returnMsg NULL if no message could be decoded, the message data else
216 * @param returnStats if a message was decoded, this is filled with the channel's stats
217 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
218 *
219 * A typical use scenario of this function call is:
220 *
221 * @code
222 * #include <mavlink.h>
223 *
224 * mavlink_message_t msg;
225 * int chan = 0;
226 *
227 *
228 * while(serial.bytesAvailable > 0)
229 * {
230 *   uint8_t byte = serial.getNextByte();
231 *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
232 *     {
233 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
234 *     }
235 * }
236 *
237 *
238 * @endcode
239 */
240MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
241                                                 mavlink_status_t* status,
242                                                 uint8_t c, 
243                                                 mavlink_message_t* r_message, 
244                                                 mavlink_status_t* r_mavlink_status)
245{
246        /*
247          default message crc function. You can override this per-system to
248          put this data in a different memory segment
249        */
250#if MAVLINK_CRC_EXTRA
251#ifndef MAVLINK_MESSAGE_CRC
252        static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
253#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
254#endif
255#endif
256
257        /* Enable this option to check the length of each message.
258           This allows invalid messages to be caught much sooner. Use if the transmission
259           medium is prone to missing (or extra) characters (e.g. a radio that fades in
260           and out). Only use if the channel will only contain messages types listed in
261           the headers.
262        */
263#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
264#ifndef MAVLINK_MESSAGE_LENGTH
265        static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
266#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
267#endif
268#endif
269
270        int bufferIndex = 0;
271
272        status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
273
274        switch (status->parse_state)
275        {
276        case MAVLINK_PARSE_STATE_UNINIT:
277        case MAVLINK_PARSE_STATE_IDLE:
278                if (c == MAVLINK_STX)
279                {
280                        status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
281                        rxmsg->len = 0;
282                        rxmsg->magic = c;
283                        mavlink_start_checksum(rxmsg);
284                }
285                break;
286
287        case MAVLINK_PARSE_STATE_GOT_STX:
288                        if (status->msg_received
289/* Support shorter buffers than the
290   default maximum packet size */
291#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
292                                || c > MAVLINK_MAX_PAYLOAD_LEN
293#endif
294                                )
295                {
296                        status->buffer_overrun++;
297                        status->parse_error++;
298                        status->msg_received = 0;
299                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
300                }
301                else
302                {
303                        // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
304                        rxmsg->len = c;
305                        status->packet_idx = 0;
306                        mavlink_update_checksum(rxmsg, c);
307                        status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
308                }
309                break;
310
311        case MAVLINK_PARSE_STATE_GOT_LENGTH:
312                rxmsg->seq = c;
313                mavlink_update_checksum(rxmsg, c);
314                status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
315                break;
316
317        case MAVLINK_PARSE_STATE_GOT_SEQ:
318                rxmsg->sysid = c;
319                mavlink_update_checksum(rxmsg, c);
320                status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
321                break;
322
323        case MAVLINK_PARSE_STATE_GOT_SYSID:
324                rxmsg->compid = c;
325                mavlink_update_checksum(rxmsg, c);
326                status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
327                break;
328
329        case MAVLINK_PARSE_STATE_GOT_COMPID:
330#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
331                if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
332                {
333                        status->parse_error++;
334                        status->parse_state = MAVLINK_PARSE_STATE_IDLE;
335                        break;
336            }
337#endif
338                rxmsg->msgid = c;
339                mavlink_update_checksum(rxmsg, c);
340                if (rxmsg->len == 0)
341                {
342                        status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
343                }
344                else
345                {
346                        status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
347                }
348                break;
349
350        case MAVLINK_PARSE_STATE_GOT_MSGID:
351                _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
352                mavlink_update_checksum(rxmsg, c);
353                if (status->packet_idx == rxmsg->len)
354                {
355                        status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
356                }
357                break;
358
359        case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
360#if MAVLINK_CRC_EXTRA
361                mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
362#endif
363                if (c != (rxmsg->checksum & 0xFF)) {
364                        status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
365                } else {
366                        status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
367                }
368                _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
369                break;
370
371        case MAVLINK_PARSE_STATE_GOT_CRC1:
372        case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
373                if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
374                        // got a bad CRC message
375                        status->msg_received = MAVLINK_FRAMING_BAD_CRC;
376                } else {
377                        // Successfully got message
378                        status->msg_received = MAVLINK_FRAMING_OK;
379                }
380                status->parse_state = MAVLINK_PARSE_STATE_IDLE;
381                _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
382                memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
383                break;
384        }
385
386        bufferIndex++;
387        // If a message has been sucessfully decoded, check index
388        if (status->msg_received == MAVLINK_FRAMING_OK)
389        {
390                //while(status->current_seq != rxmsg->seq)
391                //{
392                //      status->packet_rx_drop_count++;
393                //               status->current_seq++;
394                //}
395                status->current_rx_seq = rxmsg->seq;
396                // Initial condition: If no packet has been received so far, drop count is undefined
397                if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
398                // Count this packet as received
399                status->packet_rx_success_count++;
400        }
401
402        r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
403        r_mavlink_status->parse_state = status->parse_state;
404        r_mavlink_status->packet_idx = status->packet_idx;
405        r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
406        r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
407        r_mavlink_status->packet_rx_drop_count = status->parse_error;
408        status->parse_error = 0;
409
410        if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
411                /*
412                  the CRC came out wrong. We now need to overwrite the
413                  msg CRC with the one on the wire so that if the
414                  caller decides to forward the message anyway that
415                  mavlink_msg_to_send_buffer() won't overwrite the
416                  checksum
417                 */
418                r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
419        }
420
421        return status->msg_received;
422}
423
424/**
425 * This is a convenience function which handles the complete MAVLink parsing.
426 * the function will parse one byte at a time and return the complete packet once
427 * it could be successfully decoded. This function will return 0, 1 or
428 * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
429 *
430 * Messages are parsed into an internal buffer (one for each channel). When a complete
431 * message is received it is copies into *returnMsg and the channel's status is
432 * copied into *returnStats.
433 *
434 * @param chan     ID of the current channel. This allows to parse different channels with this function.
435 *                 a channel is not a physical message channel like a serial port, but a logic partition of
436 *                 the communication streams in this case. COMM_NB is the limit for the number of channels
437 *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
438 * @param c        The char to parse
439 *
440 * @param returnMsg NULL if no message could be decoded, the message data else
441 * @param returnStats if a message was decoded, this is filled with the channel's stats
442 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
443 *
444 * A typical use scenario of this function call is:
445 *
446 * @code
447 * #include <mavlink.h>
448 *
449 * mavlink_message_t msg;
450 * int chan = 0;
451 *
452 *
453 * while(serial.bytesAvailable > 0)
454 * {
455 *   uint8_t byte = serial.getNextByte();
456 *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
457 *     {
458 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
459 *     }
460 * }
461 *
462 *
463 * @endcode
464 */
465MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
466{
467        return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
468                                         mavlink_get_channel_status(chan),
469                                         c,
470                                         r_message,
471                                         r_mavlink_status);
472}
473
474
475/**
476 * This is a convenience function which handles the complete MAVLink parsing.
477 * the function will parse one byte at a time and return the complete packet once
478 * it could be successfully decoded. This function will return 0 or 1.
479 *
480 * Messages are parsed into an internal buffer (one for each channel). When a complete
481 * message is received it is copies into *returnMsg and the channel's status is
482 * copied into *returnStats.
483 *
484 * @param chan     ID of the current channel. This allows to parse different channels with this function.
485 *                 a channel is not a physical message channel like a serial port, but a logic partition of
486 *                 the communication streams in this case. COMM_NB is the limit for the number of channels
487 *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
488 * @param c        The char to parse
489 *
490 * @param returnMsg NULL if no message could be decoded, the message data else
491 * @param returnStats if a message was decoded, this is filled with the channel's stats
492 * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
493 *
494 * A typical use scenario of this function call is:
495 *
496 * @code
497 * #include <mavlink.h>
498 *
499 * mavlink_message_t msg;
500 * int chan = 0;
501 *
502 *
503 * while(serial.bytesAvailable > 0)
504 * {
505 *   uint8_t byte = serial.getNextByte();
506 *   if (mavlink_parse_char(chan, byte, &msg))
507 *     {
508 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
509 *     }
510 * }
511 *
512 *
513 * @endcode
514 */
515MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
516{
517    uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
518    if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
519            // we got a bad CRC. Treat as a parse failure
520            mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
521            mavlink_status_t* status = mavlink_get_channel_status(chan);
522            status->parse_error++;
523            status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
524            status->parse_state = MAVLINK_PARSE_STATE_IDLE;
525            if (c == MAVLINK_STX)
526            {
527                    status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
528                    rxmsg->len = 0;
529                    mavlink_start_checksum(rxmsg);
530            }
531            return 0;
532    }
533    return msg_received;
534}
535
536/**
537 * @brief Put a bitfield of length 1-32 bit into the buffer
538 *
539 * @param b the value to add, will be encoded in the bitfield
540 * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
541 * @param packet_index the position in the packet (the index of the first byte to use)
542 * @param bit_index the position in the byte (the index of the first bit to use)
543 * @param buffer packet buffer to write into
544 * @return new position of the last used byte in the buffer
545 */
546MAVLINK_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)
547{
548        uint16_t bits_remain = bits;
549        // Transform number into network order
550        int32_t v;
551        uint8_t i_bit_index, i_byte_index, curr_bits_n;
552#if MAVLINK_NEED_BYTE_SWAP
553        union {
554                int32_t i;
555                uint8_t b[4];
556        } bin, bout;
557        bin.i = b;
558        bout.b[0] = bin.b[3];
559        bout.b[1] = bin.b[2];
560        bout.b[2] = bin.b[1];
561        bout.b[3] = bin.b[0];
562        v = bout.i;
563#else
564        v = b;
565#endif
566
567        // buffer in
568        // 01100000 01000000 00000000 11110001
569        // buffer out
570        // 11110001 00000000 01000000 01100000
571
572        // Existing partly filled byte (four free slots)
573        // 0111xxxx
574
575        // Mask n free bits
576        // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
577        // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
578
579        // Shift n bits into the right position
580        // out = in >> n;
581
582        // Mask and shift bytes
583        i_bit_index = bit_index;
584        i_byte_index = packet_index;
585        if (bit_index > 0)
586        {
587                // If bits were available at start, they were available
588                // in the byte before the current index
589                i_byte_index--;
590        }
591
592        // While bits have not been packed yet
593        while (bits_remain > 0)
594        {
595                // Bits still have to be packed
596                // there can be more than 8 bits, so
597                // we might have to pack them into more than one byte
598
599                // First pack everything we can into the current 'open' byte
600                //curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
601                //FIXME
602                if (bits_remain <= (uint8_t)(8 - i_bit_index))
603                {
604                        // Enough space
605                        curr_bits_n = (uint8_t)bits_remain;
606                }
607                else
608                {
609                        curr_bits_n = (8 - i_bit_index);
610                }
611               
612                // Pack these n bits into the current byte
613                // Mask out whatever was at that position with ones (xxx11111)
614                buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
615                // Put content to this position, by masking out the non-used part
616                buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
617               
618                // Increment the bit index
619                i_bit_index += curr_bits_n;
620
621                // Now proceed to the next byte, if necessary
622                bits_remain -= curr_bits_n;
623                if (bits_remain > 0)
624                {
625                        // Offer another 8 bits / one byte
626                        i_byte_index++;
627                        i_bit_index = 0;
628                }
629        }
630       
631        *r_bit_index = i_bit_index;
632        // If a partly filled byte is present, mark this as consumed
633        if (i_bit_index != 7) i_byte_index++;
634        return i_byte_index - packet_index;
635}
636
637#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
638
639// To make MAVLink work on your MCU, define comm_send_ch() if you wish
640// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
641// whole packet at a time
642
643/*
644
645#include "mavlink_types.h"
646
647void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
648{
649    if (chan == MAVLINK_COMM_0)
650    {
651        uart0_transmit(ch);
652    }
653    if (chan == MAVLINK_COMM_1)
654    {
655        uart1_transmit(ch);
656    }
657}
658 */
659
660MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
661{
662#ifdef MAVLINK_SEND_UART_BYTES
663        /* this is the more efficient approach, if the platform
664           defines it */
665        MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
666#else
667        /* fallback to one byte at a time */
668        uint16_t i;
669        for (i = 0; i < len; i++) {
670                comm_send_ch(chan, (uint8_t)buf[i]);
671        }
672#endif
673}
674#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
675
676#endif /* _MAVLINK_HELPERS_H_ */
Note: See TracBrowser for help on using the repository browser.