msp: expose number of vtx power levels, bands and channels
[inav.git] / lib / main / MAVLink / mavlink_helpers.h
blobb99f41ccab49a582e09ed7abc7f4ce641debc201
1 #pragma once
3 #include "string.h"
4 #include "checksum.h"
5 #include "mavlink_types.h"
6 #include "mavlink_conversions.h"
7 #include <stdio.h>
9 #ifndef MAVLINK_HELPER
10 #define MAVLINK_HELPER
11 #endif
13 #include "mavlink_sha256.h"
15 #ifdef MAVLINK_USE_CXX_NAMESPACE
16 namespace mavlink {
17 #endif
20 * Internal function to give access to the channel status for each channel
22 #ifndef MAVLINK_GET_CHANNEL_STATUS
23 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
25 #ifdef MAVLINK_EXTERNAL_RX_STATUS
26 // No m_mavlink_status array defined in function,
27 // has to be defined externally
28 #else
29 static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
30 #endif
31 return &m_mavlink_status[chan];
33 #endif
36 * Internal function to give access to the channel buffer for each channel
38 #ifndef MAVLINK_GET_CHANNEL_BUFFER
39 MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
42 #ifdef MAVLINK_EXTERNAL_RX_BUFFER
43 // No m_mavlink_buffer array defined in function,
44 // has to be defined externally
45 #else
46 static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
47 #endif
48 return &m_mavlink_buffer[chan];
50 #endif // MAVLINK_GET_CHANNEL_BUFFER
52 /* Enable this option to check the length of each message.
53 This allows invalid messages to be caught much sooner. Use if the transmission
54 medium is prone to missing (or extra) characters (e.g. a radio that fades in
55 and out). Only use if the channel will only contain messages types listed in
56 the headers.
58 //#define MAVLINK_CHECK_MESSAGE_LENGTH
60 /**
61 * @brief Reset the status of a channel.
63 MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
65 mavlink_status_t *status = mavlink_get_channel_status(chan);
66 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
69 /**
70 * @brief create a signature block for a packet
72 MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
73 uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
74 const uint8_t *header, uint8_t header_len,
75 const uint8_t *packet, uint8_t packet_len,
76 const uint8_t crc[2])
78 mavlink_sha256_ctx ctx;
79 union {
80 uint64_t t64;
81 uint8_t t8[8];
82 } tstamp;
83 if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
84 return 0;
86 signature[0] = signing->link_id;
87 tstamp.t64 = signing->timestamp;
88 memcpy(&signature[1], tstamp.t8, 6);
89 signing->timestamp++;
91 mavlink_sha256_init(&ctx);
92 mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
93 mavlink_sha256_update(&ctx, header, header_len);
94 mavlink_sha256_update(&ctx, packet, packet_len);
95 mavlink_sha256_update(&ctx, crc, 2);
96 mavlink_sha256_update(&ctx, signature, 7);
97 mavlink_sha256_final_48(&ctx, &signature[7]);
99 return MAVLINK_SIGNATURE_BLOCK_LEN;
103 * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only).
105 * @param payload Serialised payload buffer.
106 * @param length Length of full-width payload buffer.
107 * @return Length of payload after zero-filled bytes are trimmed.
109 MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
111 while (length > 1 && payload[length-1] == 0) {
112 length--;
114 return length;
118 * @brief check a signature block for a packet
120 MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
121 mavlink_signing_streams_t *signing_streams,
122 const mavlink_message_t *msg)
124 if (signing == NULL) {
125 return true;
127 const uint8_t *p = (const uint8_t *)&msg->magic;
128 const uint8_t *psig = msg->signature;
129 const uint8_t *incoming_signature = psig+7;
130 mavlink_sha256_ctx ctx;
131 uint8_t signature[6];
132 uint16_t i;
134 mavlink_sha256_init(&ctx);
135 mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
136 mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len);
137 mavlink_sha256_update(&ctx, msg->ck, 2);
138 mavlink_sha256_update(&ctx, psig, 1+6);
139 mavlink_sha256_final_48(&ctx, signature);
140 if (memcmp(signature, incoming_signature, 6) != 0) {
141 return false;
144 // now check timestamp
145 union tstamp {
146 uint64_t t64;
147 uint8_t t8[8];
148 } tstamp;
149 uint8_t link_id = psig[0];
150 tstamp.t64 = 0;
151 memcpy(tstamp.t8, psig+1, 6);
153 if (signing_streams == NULL) {
154 return false;
157 // find stream
158 for (i=0; i<signing_streams->num_signing_streams; i++) {
159 if (msg->sysid == signing_streams->stream[i].sysid &&
160 msg->compid == signing_streams->stream[i].compid &&
161 link_id == signing_streams->stream[i].link_id) {
162 break;
165 if (i == signing_streams->num_signing_streams) {
166 if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
167 // over max number of streams
168 return false;
170 // new stream. Only accept if timestamp is not more than 1 minute old
171 if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
172 return false;
174 // add new stream
175 signing_streams->stream[i].sysid = msg->sysid;
176 signing_streams->stream[i].compid = msg->compid;
177 signing_streams->stream[i].link_id = link_id;
178 signing_streams->num_signing_streams++;
179 } else {
180 union tstamp last_tstamp;
181 last_tstamp.t64 = 0;
182 memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
183 if (tstamp.t64 <= last_tstamp.t64) {
184 // repeating old timestamp
185 return false;
189 // remember last timestamp
190 memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
192 // our next timestamp must be at least this timestamp
193 if (tstamp.t64 > signing->timestamp) {
194 signing->timestamp = tstamp.t64;
196 return true;
201 * @brief Finalize a MAVLink message with channel assignment
203 * This function calculates the checksum and sets length and aircraft id correctly.
204 * It assumes that the message id and the payload are already correctly set. This function
205 * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
206 * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
208 * @param msg Message to finalize
209 * @param system_id Id of the sending (this) system, 1-127
210 * @param length Message length
212 MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
213 mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
215 bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
216 bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
217 uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
218 uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
219 uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
220 if (mavlink1) {
221 msg->magic = MAVLINK_STX_MAVLINK1;
222 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
223 } else {
224 msg->magic = MAVLINK_STX;
226 msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
227 msg->sysid = system_id;
228 msg->compid = component_id;
229 msg->incompat_flags = 0;
230 if (signing) {
231 msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
233 msg->compat_flags = 0;
234 msg->seq = status->current_tx_seq;
235 status->current_tx_seq = status->current_tx_seq + 1;
237 // form the header as a byte array for the crc
238 buf[0] = msg->magic;
239 buf[1] = msg->len;
240 if (mavlink1) {
241 buf[2] = msg->seq;
242 buf[3] = msg->sysid;
243 buf[4] = msg->compid;
244 buf[5] = msg->msgid & 0xFF;
245 } else {
246 buf[2] = msg->incompat_flags;
247 buf[3] = msg->compat_flags;
248 buf[4] = msg->seq;
249 buf[5] = msg->sysid;
250 buf[6] = msg->compid;
251 buf[7] = msg->msgid & 0xFF;
252 buf[8] = (msg->msgid >> 8) & 0xFF;
253 buf[9] = (msg->msgid >> 16) & 0xFF;
256 uint16_t checksum = crc_calculate(&buf[1], header_len-1);
257 crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len);
258 crc_accumulate(crc_extra, &checksum);
259 mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
260 mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
262 msg->checksum = checksum;
264 if (signing) {
265 mavlink_sign_packet(status->signing,
266 msg->signature,
267 (const uint8_t *)buf, header_len,
268 (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
269 (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
272 return msg->len + header_len + 2 + signature_len;
275 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
276 uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
278 mavlink_status_t *status = mavlink_get_channel_status(chan);
279 return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra);
283 * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
285 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
286 uint8_t min_length, uint8_t length, uint8_t crc_extra)
288 return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
291 static inline void _mav_parse_error(mavlink_status_t *status)
293 status->parse_error++;
296 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
297 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
300 * @brief Finalize a MAVLink message with channel assignment and send
302 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
303 const char *packet,
304 uint8_t min_length, uint8_t length, uint8_t crc_extra)
306 uint16_t checksum;
307 uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
308 uint8_t ck[2];
309 mavlink_status_t *status = mavlink_get_channel_status(chan);
310 uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
311 uint8_t signature_len = 0;
312 uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
313 bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
314 bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
316 if (mavlink1) {
317 length = min_length;
318 if (msgid > 255) {
319 // can't send 16 bit messages
320 _mav_parse_error(status);
321 return;
323 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
324 buf[0] = MAVLINK_STX_MAVLINK1;
325 buf[1] = length;
326 buf[2] = status->current_tx_seq;
327 buf[3] = mavlink_system.sysid;
328 buf[4] = mavlink_system.compid;
329 buf[5] = msgid & 0xFF;
330 } else {
331 uint8_t incompat_flags = 0;
332 if (signing) {
333 incompat_flags |= MAVLINK_IFLAG_SIGNED;
335 length = _mav_trim_payload(packet, length);
336 buf[0] = MAVLINK_STX;
337 buf[1] = length;
338 buf[2] = incompat_flags;
339 buf[3] = 0; // compat_flags
340 buf[4] = status->current_tx_seq;
341 buf[5] = mavlink_system.sysid;
342 buf[6] = mavlink_system.compid;
343 buf[7] = msgid & 0xFF;
344 buf[8] = (msgid >> 8) & 0xFF;
345 buf[9] = (msgid >> 16) & 0xFF;
347 status->current_tx_seq++;
348 checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
349 crc_accumulate_buffer(&checksum, packet, length);
350 crc_accumulate(crc_extra, &checksum);
351 ck[0] = (uint8_t)(checksum & 0xFF);
352 ck[1] = (uint8_t)(checksum >> 8);
354 if (signing) {
355 // possibly add a signature
356 signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
357 (const uint8_t *)packet, length, ck);
360 MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
361 _mavlink_send_uart(chan, (const char *)buf, header_len+1);
362 _mavlink_send_uart(chan, packet, length);
363 _mavlink_send_uart(chan, (const char *)ck, 2);
364 if (signature_len != 0) {
365 _mavlink_send_uart(chan, (const char *)signature, signature_len);
367 MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
371 * @brief re-send a message over a uart channel
372 * this is more stack efficient than re-marshalling the message
373 * If the message is signed then the original signature is also sent
375 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
377 uint8_t ck[2];
379 ck[0] = (uint8_t)(msg->checksum & 0xFF);
380 ck[1] = (uint8_t)(msg->checksum >> 8);
381 // XXX use the right sequence here
383 uint8_t header_len;
384 uint8_t signature_len;
386 if (msg->magic == MAVLINK_STX_MAVLINK1) {
387 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
388 signature_len = 0;
389 MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
390 // we can't send the structure directly as it has extra mavlink2 elements in it
391 uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1];
392 buf[0] = msg->magic;
393 buf[1] = msg->len;
394 buf[2] = msg->seq;
395 buf[3] = msg->sysid;
396 buf[4] = msg->compid;
397 buf[5] = msg->msgid & 0xFF;
398 _mavlink_send_uart(chan, (const char*)buf, header_len);
399 } else {
400 header_len = MAVLINK_CORE_HEADER_LEN + 1;
401 signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
402 MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
403 uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1];
404 buf[0] = msg->magic;
405 buf[1] = msg->len;
406 buf[2] = msg->incompat_flags;
407 buf[3] = msg->compat_flags;
408 buf[4] = msg->seq;
409 buf[5] = msg->sysid;
410 buf[6] = msg->compid;
411 buf[7] = msg->msgid & 0xFF;
412 buf[8] = (msg->msgid >> 8) & 0xFF;
413 buf[9] = (msg->msgid >> 16) & 0xFF;
414 _mavlink_send_uart(chan, (const char *)buf, header_len);
416 _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
417 _mavlink_send_uart(chan, (const char *)ck, 2);
418 if (signature_len != 0) {
419 _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
421 MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
423 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
426 * @brief Pack a message to send it over a serial byte stream
428 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
430 uint8_t signature_len, header_len;
431 uint8_t *ck;
432 uint8_t length = msg->len;
434 if (msg->magic == MAVLINK_STX_MAVLINK1) {
435 signature_len = 0;
436 header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
437 buf[0] = msg->magic;
438 buf[1] = length;
439 buf[2] = msg->seq;
440 buf[3] = msg->sysid;
441 buf[4] = msg->compid;
442 buf[5] = msg->msgid & 0xFF;
443 memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
444 ck = buf + header_len + 1 + (uint16_t)msg->len;
445 } else {
446 length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
447 header_len = MAVLINK_CORE_HEADER_LEN;
448 buf[0] = msg->magic;
449 buf[1] = length;
450 buf[2] = msg->incompat_flags;
451 buf[3] = msg->compat_flags;
452 buf[4] = msg->seq;
453 buf[5] = msg->sysid;
454 buf[6] = msg->compid;
455 buf[7] = msg->msgid & 0xFF;
456 buf[8] = (msg->msgid >> 8) & 0xFF;
457 buf[9] = (msg->msgid >> 16) & 0xFF;
458 memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
459 ck = buf + header_len + 1 + (uint16_t)length;
460 signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
462 ck[0] = (uint8_t)(msg->checksum & 0xFF);
463 ck[1] = (uint8_t)(msg->checksum >> 8);
464 if (signature_len > 0) {
465 memcpy(&ck[2], msg->signature, signature_len);
468 return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
471 union __mavlink_bitfield {
472 uint8_t uint8;
473 int8_t int8;
474 uint16_t uint16;
475 int16_t int16;
476 uint32_t uint32;
477 int32_t int32;
481 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
483 uint16_t crcTmp = 0;
484 crc_init(&crcTmp);
485 msg->checksum = crcTmp;
488 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
490 uint16_t checksum = msg->checksum;
491 crc_accumulate(c, &checksum);
492 msg->checksum = checksum;
496 return the crc_entry value for a msgid
498 #ifndef MAVLINK_GET_MSG_ENTRY
499 MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
501 static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
503 use a bisection search to find the right entry. A perfect hash may be better
504 Note that this assumes the table is sorted by msgid
506 uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]) - 1;
507 while (low < high) {
508 uint32_t mid = (low+1+high)/2;
509 if (msgid < mavlink_message_crcs[mid].msgid) {
510 high = mid-1;
511 continue;
513 if (msgid > mavlink_message_crcs[mid].msgid) {
514 low = mid;
515 continue;
517 low = mid;
518 break;
520 if (mavlink_message_crcs[low].msgid != msgid) {
521 // msgid is not in the table
522 return NULL;
524 return &mavlink_message_crcs[low];
526 #endif // MAVLINK_GET_MSG_ENTRY
529 return the crc_extra value for a message
531 MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
533 const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
534 return e?e->crc_extra:0;
538 return the min message length
540 #define MAVLINK_HAVE_MIN_MESSAGE_LENGTH
541 MAVLINK_HELPER uint8_t mavlink_min_message_length(const mavlink_message_t *msg)
543 const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
544 return e?e->min_msg_len:0;
548 return the max message length (including extensions)
550 #define MAVLINK_HAVE_MAX_MESSAGE_LENGTH
551 MAVLINK_HELPER uint8_t mavlink_max_message_length(const mavlink_message_t *msg)
553 const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
554 return e?e->max_msg_len:0;
558 * This is a variant of mavlink_frame_char() but with caller supplied
559 * parsing buffers. It is useful when you want to create a MAVLink
560 * parser in a library that doesn't use any global variables
562 * @param rxmsg parsing message buffer
563 * @param status parsing status buffer
564 * @param c The char to parse
566 * @param r_message NULL if no message could be decoded, otherwise the message data
567 * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
568 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
571 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
572 mavlink_status_t* status,
573 uint8_t c,
574 mavlink_message_t* r_message,
575 mavlink_status_t* r_mavlink_status)
577 status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
579 switch (status->parse_state)
581 case MAVLINK_PARSE_STATE_UNINIT:
582 case MAVLINK_PARSE_STATE_IDLE:
583 if (c == MAVLINK_STX)
585 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
586 rxmsg->len = 0;
587 rxmsg->magic = c;
588 status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
589 mavlink_start_checksum(rxmsg);
590 } else if (c == MAVLINK_STX_MAVLINK1)
592 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
593 rxmsg->len = 0;
594 rxmsg->magic = c;
595 status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
596 mavlink_start_checksum(rxmsg);
598 break;
600 case MAVLINK_PARSE_STATE_GOT_STX:
601 if (status->msg_received
602 /* Support shorter buffers than the
603 default maximum packet size */
604 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
605 || c > MAVLINK_MAX_PAYLOAD_LEN
606 #endif
609 status->buffer_overrun++;
610 _mav_parse_error(status);
611 status->msg_received = 0;
612 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
614 else
616 // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
617 rxmsg->len = c;
618 status->packet_idx = 0;
619 mavlink_update_checksum(rxmsg, c);
620 if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
621 rxmsg->incompat_flags = 0;
622 rxmsg->compat_flags = 0;
623 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
624 } else {
625 status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
628 break;
630 case MAVLINK_PARSE_STATE_GOT_LENGTH:
631 rxmsg->incompat_flags = c;
632 if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
633 // message includes an incompatible feature flag
634 _mav_parse_error(status);
635 status->msg_received = 0;
636 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
637 break;
639 mavlink_update_checksum(rxmsg, c);
640 status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
641 break;
643 case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
644 rxmsg->compat_flags = c;
645 mavlink_update_checksum(rxmsg, c);
646 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
647 break;
649 case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
650 rxmsg->seq = c;
651 mavlink_update_checksum(rxmsg, c);
652 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
653 break;
655 case MAVLINK_PARSE_STATE_GOT_SEQ:
656 rxmsg->sysid = c;
657 mavlink_update_checksum(rxmsg, c);
658 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
659 break;
661 case MAVLINK_PARSE_STATE_GOT_SYSID:
662 rxmsg->compid = c;
663 mavlink_update_checksum(rxmsg, c);
664 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
665 break;
667 case MAVLINK_PARSE_STATE_GOT_COMPID:
668 rxmsg->msgid = c;
669 mavlink_update_checksum(rxmsg, c);
670 if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
671 if(rxmsg->len > 0) {
672 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
673 } else {
674 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
676 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
677 if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
678 rxmsg->len > mavlink_max_message_length(rxmsg)) {
679 _mav_parse_error(status);
680 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
681 break;
683 #endif
684 } else {
685 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
687 break;
689 case MAVLINK_PARSE_STATE_GOT_MSGID1:
690 rxmsg->msgid |= c<<8;
691 mavlink_update_checksum(rxmsg, c);
692 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
693 break;
695 case MAVLINK_PARSE_STATE_GOT_MSGID2:
696 rxmsg->msgid |= ((uint32_t)c)<<16;
697 mavlink_update_checksum(rxmsg, c);
698 if(rxmsg->len > 0){
699 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
700 } else {
701 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
703 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
704 if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
705 rxmsg->len > mavlink_max_message_length(rxmsg))
707 _mav_parse_error(status);
708 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
709 break;
711 #endif
712 break;
714 case MAVLINK_PARSE_STATE_GOT_MSGID3:
715 _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
716 mavlink_update_checksum(rxmsg, c);
717 if (status->packet_idx == rxmsg->len)
719 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
721 break;
723 case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
724 const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
725 uint8_t crc_extra = e?e->crc_extra:0;
726 mavlink_update_checksum(rxmsg, crc_extra);
727 if (c != (rxmsg->checksum & 0xFF)) {
728 status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
729 } else {
730 status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
732 rxmsg->ck[0] = c;
734 // zero-fill the packet to cope with short incoming packets
735 if (e && status->packet_idx < e->max_msg_len) {
736 memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);
738 break;
741 case MAVLINK_PARSE_STATE_GOT_CRC1:
742 case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
743 if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
744 // got a bad CRC message
745 status->msg_received = MAVLINK_FRAMING_BAD_CRC;
746 } else {
747 // Successfully got message
748 status->msg_received = MAVLINK_FRAMING_OK;
750 rxmsg->ck[1] = c;
752 if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
753 status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
754 status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
756 // If the CRC is already wrong, don't overwrite msg_received,
757 // otherwise we can end up with garbage flagged as valid.
758 if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
759 status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
761 } else {
762 if (status->signing &&
763 (status->signing->accept_unsigned_callback == NULL ||
764 !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
766 // If the CRC is already wrong, don't overwrite msg_received.
767 if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
768 status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
771 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
772 if (r_message != NULL) {
773 memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
776 break;
777 case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
778 rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
779 status->signature_wait--;
780 if (status->signature_wait == 0) {
781 // we have the whole signature, check it is OK
782 bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
783 if (!sig_ok &&
784 (status->signing->accept_unsigned_callback &&
785 status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
786 // accepted via application level override
787 sig_ok = true;
789 if (sig_ok) {
790 status->msg_received = MAVLINK_FRAMING_OK;
791 } else {
792 status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
794 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
795 if (r_message !=NULL) {
796 memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
799 break;
802 // If a message has been sucessfully decoded, check index
803 if (status->msg_received == MAVLINK_FRAMING_OK)
805 //while(status->current_seq != rxmsg->seq)
807 // status->packet_rx_drop_count++;
808 // status->current_seq++;
810 status->current_rx_seq = rxmsg->seq;
811 // Initial condition: If no packet has been received so far, drop count is undefined
812 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
813 // Count this packet as received
814 status->packet_rx_success_count++;
817 if (r_message != NULL) {
818 r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
820 if (r_mavlink_status != NULL) {
821 r_mavlink_status->parse_state = status->parse_state;
822 r_mavlink_status->packet_idx = status->packet_idx;
823 r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
824 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
825 r_mavlink_status->packet_rx_drop_count = status->parse_error;
826 r_mavlink_status->flags = status->flags;
828 status->parse_error = 0;
830 if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
832 the CRC came out wrong. We now need to overwrite the
833 msg CRC with the one on the wire so that if the
834 caller decides to forward the message anyway that
835 mavlink_msg_to_send_buffer() won't overwrite the
836 checksum
838 if (r_message != NULL) {
839 r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
843 return status->msg_received;
847 * This is a convenience function which handles the complete MAVLink parsing.
848 * the function will parse one byte at a time and return the complete packet once
849 * it could be successfully decoded. This function will return 0, 1 or
850 * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
852 * Messages are parsed into an internal buffer (one for each channel). When a complete
853 * message is received it is copies into *r_message and the channel's status is
854 * copied into *r_mavlink_status.
856 * @param chan ID of the channel to be parsed.
857 * A channel is not a physical message channel like a serial port, but a logical partition of
858 * the communication streams. COMM_NB is the limit for the number of channels
859 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
860 * @param c The char to parse
862 * @param r_message NULL if no message could be decoded, otherwise the message data
863 * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
864 * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
866 * A typical use scenario of this function call is:
868 * @code
869 * #include <mavlink.h>
871 * mavlink_status_t status;
872 * mavlink_message_t msg;
873 * int chan = 0;
876 * while(serial.bytesAvailable > 0)
878 * uint8_t byte = serial.getNextByte();
879 * if (mavlink_frame_char(chan, byte, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE)
881 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
886 * @endcode
888 MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
890 return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
891 mavlink_get_channel_status(chan),
893 r_message,
894 r_mavlink_status);
898 * Set the protocol version
900 MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version)
902 mavlink_status_t *status = mavlink_get_channel_status(chan);
903 if (version > 1) {
904 status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
905 } else {
906 status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
911 * Get the protocol version
913 * @return 1 for v1, 2 for v2
915 MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan)
917 mavlink_status_t *status = mavlink_get_channel_status(chan);
918 if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) {
919 return 1;
920 } else {
921 return 2;
926 * This is a convenience function which handles the complete MAVLink parsing.
927 * the function will parse one byte at a time and return the complete packet once
928 * it could be successfully decoded. This function will return 0 or 1.
930 * Messages are parsed into an internal buffer (one for each channel). When a complete
931 * message is received it is copies into *r_message and the channel's status is
932 * copied into *r_mavlink_status.
934 * @param chan ID of the channel to be parsed.
935 * A channel is not a physical message channel like a serial port, but a logical partition of
936 * the communication streams. COMM_NB is the limit for the number of channels
937 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
938 * @param c The char to parse
940 * @param r_message NULL if no message could be decoded, otherwise the message data
941 * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
942 * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
944 * A typical use scenario of this function call is:
946 * @code
947 * #include <mavlink.h>
949 * mavlink_status_t status;
950 * mavlink_message_t msg;
951 * int chan = 0;
954 * while(serial.bytesAvailable > 0)
956 * uint8_t byte = serial.getNextByte();
957 * if (mavlink_parse_char(chan, byte, &msg, &status))
959 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
964 * @endcode
966 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
968 uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
969 if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
970 msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
971 // we got a bad CRC. Treat as a parse failure
972 mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
973 mavlink_status_t* status = mavlink_get_channel_status(chan);
974 _mav_parse_error(status);
975 status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
976 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
977 if (c == MAVLINK_STX)
979 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
980 rxmsg->len = 0;
981 mavlink_start_checksum(rxmsg);
983 return 0;
985 return msg_received;
989 * @brief Put a bitfield of length 1-32 bit into the buffer
991 * @param b the value to add, will be encoded in the bitfield
992 * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
993 * @param packet_index the position in the packet (the index of the first byte to use)
994 * @param bit_index the position in the byte (the index of the first bit to use)
995 * @param buffer packet buffer to write into
996 * @return new position of the last used byte in the buffer
998 MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
1000 uint16_t bits_remain = bits;
1001 // Transform number into network order
1002 int32_t v;
1003 uint8_t i_bit_index, i_byte_index, curr_bits_n;
1004 #if MAVLINK_NEED_BYTE_SWAP
1005 union {
1006 int32_t i;
1007 uint8_t b[4];
1008 } bin, bout;
1009 bin.i = b;
1010 bout.b[0] = bin.b[3];
1011 bout.b[1] = bin.b[2];
1012 bout.b[2] = bin.b[1];
1013 bout.b[3] = bin.b[0];
1014 v = bout.i;
1015 #else
1016 v = b;
1017 #endif
1019 // buffer in
1020 // 01100000 01000000 00000000 11110001
1021 // buffer out
1022 // 11110001 00000000 01000000 01100000
1024 // Existing partly filled byte (four free slots)
1025 // 0111xxxx
1027 // Mask n free bits
1028 // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
1029 // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
1031 // Shift n bits into the right position
1032 // out = in >> n;
1034 // Mask and shift bytes
1035 i_bit_index = bit_index;
1036 i_byte_index = packet_index;
1037 if (bit_index > 0)
1039 // If bits were available at start, they were available
1040 // in the byte before the current index
1041 i_byte_index--;
1044 // While bits have not been packed yet
1045 while (bits_remain > 0)
1047 // Bits still have to be packed
1048 // there can be more than 8 bits, so
1049 // we might have to pack them into more than one byte
1051 // First pack everything we can into the current 'open' byte
1052 //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
1053 //FIXME
1054 if (bits_remain <= (uint8_t)(8 - i_bit_index))
1056 // Enough space
1057 curr_bits_n = (uint8_t)bits_remain;
1059 else
1061 curr_bits_n = (8 - i_bit_index);
1064 // Pack these n bits into the current byte
1065 // Mask out whatever was at that position with ones (xxx11111)
1066 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
1067 // Put content to this position, by masking out the non-used part
1068 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
1070 // Increment the bit index
1071 i_bit_index += curr_bits_n;
1073 // Now proceed to the next byte, if necessary
1074 bits_remain -= curr_bits_n;
1075 if (bits_remain > 0)
1077 // Offer another 8 bits / one byte
1078 i_byte_index++;
1079 i_bit_index = 0;
1083 *r_bit_index = i_bit_index;
1084 // If a partly filled byte is present, mark this as consumed
1085 if (i_bit_index != 7) i_byte_index++;
1086 return i_byte_index - packet_index;
1089 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
1091 // To make MAVLink work on your MCU, define comm_send_ch() if you wish
1092 // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
1093 // whole packet at a time
1097 #include "mavlink_types.h"
1099 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
1101 if (chan == MAVLINK_COMM_0)
1103 uart0_transmit(ch);
1105 if (chan == MAVLINK_COMM_1)
1107 uart1_transmit(ch);
1112 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
1114 #ifdef MAVLINK_SEND_UART_BYTES
1115 /* this is the more efficient approach, if the platform
1116 defines it */
1117 MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
1118 #else
1119 /* fallback to one byte at a time */
1120 uint16_t i;
1121 for (i = 0; i < len; i++) {
1122 comm_send_ch(chan, (uint8_t)buf[i]);
1124 #endif
1126 #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
1128 #ifdef MAVLINK_USE_CXX_NAMESPACE
1129 } // namespace mavlink
1130 #endif