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, 8 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.