1#![cfg_attr(not(feature = "std"), no_std)]
22#![cfg_attr(all(any(docsrs, doc), not(doctest)), feature(doc_auto_cfg))]
23#![deny(clippy::all)]
24#![warn(clippy::use_self)]
25
26use core::result::Result;
27
28#[cfg(feature = "std")]
29use std::io::{Read, Write};
30
31pub mod utils;
32#[allow(unused_imports)]
33use utils::{remove_trailing_zeroes, RustDefault};
34
35#[cfg(feature = "serde")]
36use serde::{Deserialize, Serialize};
37
38pub mod peek_reader;
39use peek_reader::PeekReader;
40
41use crate::{bytes::Bytes, error::ParserError};
42
43use crc_any::CRCu16;
44
45pub mod bytes;
46pub mod bytes_mut;
47#[cfg(feature = "std")]
48mod connection;
49pub mod error;
50#[cfg(feature = "std")]
51pub use self::connection::{connect, Connectable, MavConnection};
52
53#[cfg(feature = "tokio-1")]
54mod async_connection;
55#[cfg(feature = "tokio-1")]
56pub use self::async_connection::{connect_async, AsyncConnectable, AsyncMavConnection};
57
58#[cfg(feature = "tokio-1")]
59pub mod async_peek_reader;
60#[cfg(feature = "tokio-1")]
61use async_peek_reader::AsyncPeekReader;
62
63#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
64pub mod embedded;
65#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
66use embedded::{Read, Write};
67
68#[cfg(not(feature = "signing"))]
69type SigningData = ();
70#[cfg(feature = "signing")]
71mod signing;
72#[cfg(feature = "signing")]
73pub use self::signing::{SigningConfig, SigningData};
74#[cfg(feature = "signing")]
75use sha2::{Digest, Sha256};
76
77#[cfg(any(feature = "std", feature = "tokio-1"))]
78mod connectable;
79#[cfg(any(feature = "std", feature = "tokio-1"))]
80pub use connectable::{
81 ConnectionAddress, FileConnectable, SerialConnectable, TcpConnectable, UdpConnectable,
82};
83
84pub const MAX_FRAME_SIZE: usize = 280;
85
86pub trait Message
87where
88 Self: Sized,
89{
90 fn message_id(&self) -> u32;
91 fn message_name(&self) -> &'static str;
92
93 fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize;
95
96 fn parse(
97 version: MavlinkVersion,
98 msgid: u32,
99 payload: &[u8],
100 ) -> Result<Self, error::ParserError>;
101
102 fn message_id_from_name(name: &str) -> Result<u32, &'static str>;
103 fn default_message_from_id(id: u32) -> Result<Self, &'static str>;
104 fn extra_crc(id: u32) -> u8;
105}
106
107pub trait MessageData: Sized {
108 type Message: Message;
109
110 const ID: u32;
111 const NAME: &'static str;
112 const EXTRA_CRC: u8;
113 const ENCODED_LEN: usize;
114
115 fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize;
116 fn deser(version: MavlinkVersion, payload: &[u8]) -> Result<Self, ParserError>;
117}
118
119#[derive(Debug, Copy, Clone, PartialEq, Eq)]
121#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
122pub struct MavHeader {
123 pub system_id: u8,
124 pub component_id: u8,
125 pub sequence: u8,
126}
127
128#[derive(Debug, Copy, Clone, PartialEq, Eq)]
130#[cfg_attr(feature = "serde", derive(Serialize))]
131#[cfg_attr(feature = "serde", serde(tag = "type"))]
132pub enum MavlinkVersion {
133 V1,
134 V2,
135}
136
137pub const MAV_STX: u8 = 0xFE;
139
140pub const MAV_STX_V2: u8 = 0xFD;
142
143impl Default for MavHeader {
146 fn default() -> Self {
147 Self {
148 system_id: 255,
149 component_id: 0,
150 sequence: 0,
151 }
152 }
153}
154
155#[derive(Debug, Clone)]
159#[cfg_attr(feature = "serde", derive(Serialize))]
160pub struct MavFrame<M: Message> {
161 pub header: MavHeader,
162 pub msg: M,
163 pub protocol_version: MavlinkVersion,
164}
165
166impl<M: Message> MavFrame<M> {
167 pub fn ser(&self, buf: &mut [u8]) -> usize {
171 let mut buf = bytes_mut::BytesMut::new(buf);
172
173 let mut payload_buf = [0u8; 255];
175 let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
176
177 buf.put_u8(self.header.sequence);
196 buf.put_u8(self.header.system_id);
197 buf.put_u8(self.header.component_id);
198
199 match self.protocol_version {
201 MavlinkVersion::V2 => {
202 let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
203 buf.put_slice(&bytes[..3]);
204 }
205 MavlinkVersion::V1 => {
206 buf.put_u8(self.msg.message_id() as u8); }
208 }
209
210 buf.put_slice(&payload_buf[..payload_len]);
211 buf.len()
212 }
213
214 pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result<Self, ParserError> {
218 let mut buf = Bytes::new(input);
219
220 let sequence = buf.get_u8();
230 let system_id = buf.get_u8();
231 let component_id = buf.get_u8();
232 let header = MavHeader {
233 system_id,
234 component_id,
235 sequence,
236 };
237
238 let msg_id = match version {
239 MavlinkVersion::V2 => buf.get_u24_le(),
240 MavlinkVersion::V1 => buf.get_u8().into(),
241 };
242
243 M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self {
244 header,
245 msg,
246 protocol_version: version,
247 })
248 }
249
250 pub fn header(&self) -> MavHeader {
252 self.header
253 }
254}
255
256pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 {
257 let mut crc_calculator = CRCu16::crc16mcrf4cc();
258 crc_calculator.digest(data);
259
260 crc_calculator.digest(&[extra_crc]);
261 crc_calculator.get_crc()
262}
263
264pub fn read_versioned_msg<M: Message, R: Read>(
265 r: &mut PeekReader<R>,
266 version: MavlinkVersion,
267) -> Result<(MavHeader, M), error::MessageReadError> {
268 match version {
269 MavlinkVersion::V2 => read_v2_msg(r),
270 MavlinkVersion::V1 => read_v1_msg(r),
271 }
272}
273
274#[cfg(feature = "tokio-1")]
275pub async fn read_versioned_msg_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
276 r: &mut AsyncPeekReader<R>,
277 version: MavlinkVersion,
278) -> Result<(MavHeader, M), error::MessageReadError> {
279 match version {
280 MavlinkVersion::V2 => read_v2_msg_async(r).await,
281 MavlinkVersion::V1 => read_v1_msg_async(r).await,
282 }
283}
284
285#[cfg(feature = "signing")]
286pub fn read_versioned_msg_signed<M: Message, R: Read>(
287 r: &mut PeekReader<R>,
288 version: MavlinkVersion,
289 signing_data: Option<&SigningData>,
290) -> Result<(MavHeader, M), error::MessageReadError> {
291 match version {
292 MavlinkVersion::V2 => read_v2_msg_inner(r, signing_data),
293 MavlinkVersion::V1 => read_v1_msg(r),
294 }
295}
296
297#[cfg(all(feature = "tokio-1", feature = "signing"))]
298pub async fn read_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
299 r: &mut AsyncPeekReader<R>,
300 version: MavlinkVersion,
301 signing_data: Option<&SigningData>,
302) -> Result<(MavHeader, M), error::MessageReadError> {
303 match version {
304 MavlinkVersion::V2 => read_v2_msg_async_inner(r, signing_data).await,
305 MavlinkVersion::V1 => read_v1_msg_async(r).await,
306 }
307}
308
309#[derive(Debug, Copy, Clone, PartialEq, Eq)]
310pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]);
312
313impl Default for MAVLinkV1MessageRaw {
314 fn default() -> Self {
315 Self::new()
316 }
317}
318
319impl MAVLinkV1MessageRaw {
320 const HEADER_SIZE: usize = 5;
321
322 pub const fn new() -> Self {
323 Self([0; 1 + Self::HEADER_SIZE + 255 + 2])
324 }
325
326 #[inline]
327 pub fn header(&self) -> &[u8] {
328 &self.0[1..=Self::HEADER_SIZE]
329 }
330
331 #[inline]
332 fn mut_header(&mut self) -> &mut [u8] {
333 &mut self.0[1..=Self::HEADER_SIZE]
334 }
335
336 #[inline]
337 pub fn payload_length(&self) -> u8 {
338 self.0[1]
339 }
340
341 #[inline]
342 pub fn sequence(&self) -> u8 {
343 self.0[2]
344 }
345
346 #[inline]
347 pub fn system_id(&self) -> u8 {
348 self.0[3]
349 }
350
351 #[inline]
352 pub fn component_id(&self) -> u8 {
353 self.0[4]
354 }
355
356 #[inline]
357 pub fn message_id(&self) -> u8 {
358 self.0[5]
359 }
360
361 #[inline]
362 pub fn payload(&self) -> &[u8] {
363 let payload_length: usize = self.payload_length().into();
364 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
365 }
366
367 #[inline]
368 pub fn checksum(&self) -> u16 {
369 let payload_length: usize = self.payload_length().into();
370 u16::from_le_bytes([
371 self.0[1 + Self::HEADER_SIZE + payload_length],
372 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
373 ])
374 }
375
376 #[inline]
377 fn mut_payload_and_checksum(&mut self) -> &mut [u8] {
378 let payload_length: usize = self.payload_length().into();
379 &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)]
380 }
381
382 #[inline]
383 pub fn has_valid_crc<M: Message>(&self) -> bool {
384 let payload_length: usize = self.payload_length().into();
385 self.checksum()
386 == calculate_crc(
387 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
388 M::extra_crc(self.message_id().into()),
389 )
390 }
391
392 pub fn raw_bytes(&self) -> &[u8] {
393 let payload_length = self.payload_length() as usize;
394 &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)]
395 }
396
397 fn serialize_stx_and_header_and_crc(
398 &mut self,
399 header: MavHeader,
400 msgid: u32,
401 payload_length: usize,
402 extra_crc: u8,
403 ) {
404 self.0[0] = MAV_STX;
405
406 let header_buf = self.mut_header();
407 header_buf.copy_from_slice(&[
408 payload_length as u8,
409 header.sequence,
410 header.system_id,
411 header.component_id,
412 msgid as u8,
413 ]);
414
415 let crc = calculate_crc(
416 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
417 extra_crc,
418 );
419 self.0[(1 + Self::HEADER_SIZE + payload_length)
420 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
421 .copy_from_slice(&crc.to_le_bytes());
422 }
423
424 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
425 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
426 let payload_length = message.ser(MavlinkVersion::V1, payload_buf);
427
428 let message_id = message.message_id();
429 self.serialize_stx_and_header_and_crc(
430 header,
431 message_id,
432 payload_length,
433 M::extra_crc(message_id),
434 );
435 }
436
437 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
438 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
439 let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf);
440
441 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC);
442 }
443}
444
445pub fn read_v1_raw_message<M: Message, R: Read>(
448 reader: &mut PeekReader<R>,
449) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
450 loop {
451 while reader.peek_exact(1)?[0] != MAV_STX {
453 reader.consume(1);
454 }
455
456 let mut message = MAVLinkV1MessageRaw::new();
457 let whole_header_size = MAVLinkV1MessageRaw::HEADER_SIZE + 1;
458
459 message.0[0] = MAV_STX;
460 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
461 message.mut_header().copy_from_slice(header);
462 let packet_length = message.raw_bytes().len();
463 let payload_and_checksum =
464 &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
465 message
466 .mut_payload_and_checksum()
467 .copy_from_slice(payload_and_checksum);
468
469 if message.has_valid_crc::<M>() {
472 reader.consume(message.raw_bytes().len());
473 return Ok(message);
474 }
475
476 reader.consume(1);
477 }
478}
479
480#[cfg(feature = "tokio-1")]
483pub async fn read_v1_raw_message_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
484 reader: &mut AsyncPeekReader<R>,
485) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
486 loop {
487 loop {
488 if reader.read_u8().await? == MAV_STX {
490 break;
491 }
492 }
493
494 let mut message = MAVLinkV1MessageRaw::new();
495
496 message.0[0] = MAV_STX;
497 let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await?
498 [..MAVLinkV1MessageRaw::HEADER_SIZE];
499 message.mut_header().copy_from_slice(header);
500 let packet_length = message.raw_bytes().len() - 1;
501 let payload_and_checksum = &reader.peek_exact(packet_length).await?
502 [MAVLinkV1MessageRaw::HEADER_SIZE..packet_length];
503 message
504 .mut_payload_and_checksum()
505 .copy_from_slice(payload_and_checksum);
506
507 if message.has_valid_crc::<M>() {
510 reader.consume(message.raw_bytes().len() - 1);
511 return Ok(message);
512 }
513 }
514}
515
516#[cfg(feature = "embedded")]
522pub async fn read_v1_raw_message_async<M: Message>(
523 reader: &mut impl embedded_io_async::Read,
524) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
525 loop {
526 let mut byte = [0u8];
528 loop {
529 reader
530 .read_exact(&mut byte)
531 .await
532 .map_err(|_| error::MessageReadError::Io)?;
533 if byte[0] == MAV_STX {
534 break;
535 }
536 }
537
538 let mut message = MAVLinkV1MessageRaw::new();
539
540 message.0[0] = MAV_STX;
541 reader
542 .read_exact(message.mut_header())
543 .await
544 .map_err(|_| error::MessageReadError::Io)?;
545 reader
546 .read_exact(message.mut_payload_and_checksum())
547 .await
548 .map_err(|_| error::MessageReadError::Io)?;
549
550 if message.has_valid_crc::<M>() {
553 return Ok(message);
554 }
555 }
556}
557
558pub fn read_v1_msg<M: Message, R: Read>(
560 r: &mut PeekReader<R>,
561) -> Result<(MavHeader, M), error::MessageReadError> {
562 let message = read_v1_raw_message::<M, _>(r)?;
563
564 Ok((
565 MavHeader {
566 sequence: message.sequence(),
567 system_id: message.system_id(),
568 component_id: message.component_id(),
569 },
570 M::parse(
571 MavlinkVersion::V1,
572 u32::from(message.message_id()),
573 message.payload(),
574 )?,
575 ))
576}
577
578#[cfg(feature = "tokio-1")]
580pub async fn read_v1_msg_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
581 r: &mut AsyncPeekReader<R>,
582) -> Result<(MavHeader, M), error::MessageReadError> {
583 let message = read_v1_raw_message_async::<M, _>(r).await?;
584
585 Ok((
586 MavHeader {
587 sequence: message.sequence(),
588 system_id: message.system_id(),
589 component_id: message.component_id(),
590 },
591 M::parse(
592 MavlinkVersion::V1,
593 u32::from(message.message_id()),
594 message.payload(),
595 )?,
596 ))
597}
598
599#[cfg(feature = "embedded")]
604pub async fn read_v1_msg_async<M: Message>(
605 r: &mut impl embedded_io_async::Read,
606) -> Result<(MavHeader, M), error::MessageReadError> {
607 let message = read_v1_raw_message_async::<M>(r).await?;
608
609 Ok((
610 MavHeader {
611 sequence: message.sequence(),
612 system_id: message.system_id(),
613 component_id: message.component_id(),
614 },
615 M::parse(
616 MavlinkVersion::V1,
617 u32::from(message.message_id()),
618 message.payload(),
619 )?,
620 ))
621}
622
623const MAVLINK_IFLAG_SIGNED: u8 = 0x01;
624const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED;
625
626#[derive(Debug, Copy, Clone, PartialEq, Eq)]
627pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]);
629
630impl Default for MAVLinkV2MessageRaw {
631 fn default() -> Self {
632 Self::new()
633 }
634}
635
636impl MAVLinkV2MessageRaw {
637 const HEADER_SIZE: usize = 9;
638 const SIGNATURE_SIZE: usize = 13;
639
640 pub const fn new() -> Self {
641 Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE])
642 }
643
644 #[inline]
645 pub fn header(&self) -> &[u8] {
646 &self.0[1..=Self::HEADER_SIZE]
647 }
648
649 #[inline]
650 fn mut_header(&mut self) -> &mut [u8] {
651 &mut self.0[1..=Self::HEADER_SIZE]
652 }
653
654 #[inline]
655 pub fn payload_length(&self) -> u8 {
656 self.0[1]
657 }
658
659 #[inline]
660 pub fn incompatibility_flags(&self) -> u8 {
661 self.0[2]
662 }
663
664 #[inline]
665 pub fn incompatibility_flags_mut(&mut self) -> &mut u8 {
666 &mut self.0[2]
667 }
668
669 #[inline]
670 pub fn compatibility_flags(&self) -> u8 {
671 self.0[3]
672 }
673
674 #[inline]
675 pub fn sequence(&self) -> u8 {
676 self.0[4]
677 }
678
679 #[inline]
680 pub fn system_id(&self) -> u8 {
681 self.0[5]
682 }
683
684 #[inline]
685 pub fn component_id(&self) -> u8 {
686 self.0[6]
687 }
688
689 #[inline]
690 pub fn message_id(&self) -> u32 {
691 u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0])
692 }
693
694 #[inline]
695 pub fn payload(&self) -> &[u8] {
696 let payload_length: usize = self.payload_length().into();
697 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
698 }
699
700 #[inline]
701 pub fn checksum(&self) -> u16 {
702 let payload_length: usize = self.payload_length().into();
703 u16::from_le_bytes([
704 self.0[1 + Self::HEADER_SIZE + payload_length],
705 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
706 ])
707 }
708
709 #[cfg(feature = "signing")]
710 #[inline]
711 pub fn checksum_bytes(&self) -> &[u8] {
712 let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize;
713 &self.0[checksum_offset..(checksum_offset + 2)]
714 }
715
716 #[cfg(feature = "signing")]
717 #[inline]
718 pub fn signature_link_id(&self) -> u8 {
719 let payload_length: usize = self.payload_length().into();
720 self.0[1 + Self::HEADER_SIZE + payload_length + 2]
721 }
722
723 #[cfg(feature = "signing")]
724 #[inline]
725 pub fn signature_link_id_mut(&mut self) -> &mut u8 {
726 let payload_length: usize = self.payload_length().into();
727 &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2]
728 }
729
730 #[cfg(feature = "signing")]
731 #[inline]
732 pub fn signature_timestamp_bytes(&self) -> &[u8] {
733 let payload_length: usize = self.payload_length().into();
734 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
735 &self.0[timestamp_start..(timestamp_start + 6)]
736 }
737
738 #[cfg(feature = "signing")]
739 #[inline]
740 pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] {
741 let payload_length: usize = self.payload_length().into();
742 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
743 &mut self.0[timestamp_start..(timestamp_start + 6)]
744 }
745
746 #[cfg(feature = "signing")]
747 #[inline]
748 pub fn signature_timestamp(&self) -> u64 {
749 let mut timestamp_bytes = [0u8; 8];
750 timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes());
751 u64::from_le_bytes(timestamp_bytes)
752 }
753
754 #[cfg(feature = "signing")]
755 #[inline]
756 pub fn signature_value(&self) -> &[u8] {
757 let payload_length: usize = self.payload_length().into();
758 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
759 &self.0[signature_start..(signature_start + 6)]
760 }
761
762 #[cfg(feature = "signing")]
763 #[inline]
764 pub fn signature_value_mut(&mut self) -> &mut [u8] {
765 let payload_length: usize = self.payload_length().into();
766 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
767 &mut self.0[signature_start..(signature_start + 6)]
768 }
769
770 fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] {
771 let payload_length: usize = self.payload_length().into();
772
773 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
775 0
776 } else {
777 Self::SIGNATURE_SIZE
778 };
779
780 &mut self.0
781 [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
782 }
783
784 #[inline]
785 pub fn has_valid_crc<M: Message>(&self) -> bool {
786 let payload_length: usize = self.payload_length().into();
787 self.checksum()
788 == calculate_crc(
789 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
790 M::extra_crc(self.message_id()),
791 )
792 }
793
794 #[cfg(feature = "signing")]
795 pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) {
796 let mut hasher = Sha256::new();
797 hasher.update(secret_key);
798 hasher.update([MAV_STX_V2]);
799 hasher.update(self.header());
800 hasher.update(self.payload());
801 hasher.update(self.checksum_bytes());
802 hasher.update([self.signature_link_id()]);
803 hasher.update(self.signature_timestamp_bytes());
804 target_buffer.copy_from_slice(&hasher.finalize()[0..6]);
805 }
806
807 pub fn raw_bytes(&self) -> &[u8] {
808 let payload_length = self.payload_length() as usize;
809
810 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
811 0
812 } else {
813 Self::SIGNATURE_SIZE
814 };
815
816 &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
817 }
818
819 fn serialize_stx_and_header_and_crc(
820 &mut self,
821 header: MavHeader,
822 msgid: u32,
823 payload_length: usize,
824 extra_crc: u8,
825 incompat_flags: u8,
826 ) {
827 self.0[0] = MAV_STX_V2;
828 let msgid_bytes = msgid.to_le_bytes();
829
830 let header_buf = self.mut_header();
831 header_buf.copy_from_slice(&[
832 payload_length as u8,
833 incompat_flags,
834 0, header.sequence,
836 header.system_id,
837 header.component_id,
838 msgid_bytes[0],
839 msgid_bytes[1],
840 msgid_bytes[2],
841 ]);
842
843 let crc = calculate_crc(
844 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
845 extra_crc,
846 );
847 self.0[(1 + Self::HEADER_SIZE + payload_length)
848 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
849 .copy_from_slice(&crc.to_le_bytes());
850 }
851
852 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
853 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
854 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
855
856 let message_id = message.message_id();
857 self.serialize_stx_and_header_and_crc(
858 header,
859 message_id,
860 payload_length,
861 M::extra_crc(message_id),
862 0,
863 );
864 }
865
866 pub fn serialize_message_for_signing<M: Message>(&mut self, header: MavHeader, message: &M) {
867 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
868 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
869
870 let message_id = message.message_id();
871 self.serialize_stx_and_header_and_crc(
872 header,
873 message_id,
874 payload_length,
875 M::extra_crc(message_id),
876 0x01,
877 );
878 }
879
880 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
881 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
882 let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf);
883
884 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0);
885 }
886}
887
888#[inline]
892pub fn read_v2_raw_message<M: Message, R: Read>(
893 reader: &mut PeekReader<R>,
894) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
895 read_v2_raw_message_inner::<M, R>(reader, None)
896}
897
898#[cfg(feature = "signing")]
902#[inline]
903pub fn read_v2_raw_message_signed<M: Message, R: Read>(
904 reader: &mut PeekReader<R>,
905 signing_data: Option<&SigningData>,
906) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
907 read_v2_raw_message_inner::<M, R>(reader, signing_data)
908}
909
910#[allow(unused_variables)]
911fn read_v2_raw_message_inner<M: Message, R: Read>(
912 reader: &mut PeekReader<R>,
913 signing_data: Option<&SigningData>,
914) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
915 loop {
916 while reader.peek_exact(1)?[0] != MAV_STX_V2 {
918 reader.consume(1);
919 }
920
921 let mut message = MAVLinkV2MessageRaw::new();
922 let whole_header_size = MAVLinkV2MessageRaw::HEADER_SIZE + 1;
923
924 message.0[0] = MAV_STX_V2;
925 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
926 message.mut_header().copy_from_slice(header);
927
928 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
929 reader.consume(1);
931 continue;
932 }
933
934 let packet_length = message.raw_bytes().len();
935 let payload_and_checksum_and_sign =
936 &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
937 message
938 .mut_payload_and_checksum_and_sign()
939 .copy_from_slice(payload_and_checksum_and_sign);
940
941 if message.has_valid_crc::<M>() {
942 reader.consume(message.raw_bytes().len());
944 } else {
945 reader.consume(1);
946 continue;
947 }
948
949 #[cfg(feature = "signing")]
950 if let Some(signing_data) = signing_data {
951 if !signing_data.verify_signature(&message) {
952 continue;
953 }
954 }
955
956 return Ok(message);
957 }
958}
959
960#[cfg(feature = "tokio-1")]
963pub async fn read_v2_raw_message_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
964 reader: &mut AsyncPeekReader<R>,
965) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
966 read_v2_raw_message_async_inner::<M, R>(reader, None).await
967}
968
969#[cfg(feature = "tokio-1")]
972#[allow(unused_variables)]
973async fn read_v2_raw_message_async_inner<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
974 reader: &mut AsyncPeekReader<R>,
975 signing_data: Option<&SigningData>,
976) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
977 loop {
978 loop {
979 if reader.read_u8().await? == MAV_STX_V2 {
981 break;
982 }
983 }
984
985 let mut message = MAVLinkV2MessageRaw::new();
986
987 message.0[0] = MAV_STX_V2;
988 let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE).await?
989 [..MAVLinkV2MessageRaw::HEADER_SIZE];
990 message.mut_header().copy_from_slice(header);
991
992 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
993 continue;
995 }
996
997 let packet_length = message.raw_bytes().len() - 1;
998 let payload_and_checksum_and_sign = &reader.peek_exact(packet_length).await?
999 [MAVLinkV2MessageRaw::HEADER_SIZE..packet_length];
1000 message
1001 .mut_payload_and_checksum_and_sign()
1002 .copy_from_slice(payload_and_checksum_and_sign);
1003
1004 if message.has_valid_crc::<M>() {
1005 reader.consume(message.raw_bytes().len() - 1);
1007 } else {
1008 continue;
1009 }
1010
1011 #[cfg(feature = "signing")]
1012 if let Some(signing_data) = signing_data {
1013 if !signing_data.verify_signature(&message) {
1014 continue;
1015 }
1016 }
1017
1018 return Ok(message);
1019 }
1020}
1021
1022#[cfg(all(feature = "tokio-1", feature = "signing"))]
1025pub async fn read_v2_raw_message_async_signed<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
1026 reader: &mut AsyncPeekReader<R>,
1027 signing_data: Option<&SigningData>,
1028) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1029 read_v2_raw_message_async_inner::<M, R>(reader, signing_data).await
1030}
1031
1032#[cfg(feature = "embedded")]
1038pub async fn read_v2_raw_message_async<M: Message>(
1039 reader: &mut impl embedded_io_async::Read,
1040) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1041 loop {
1042 let mut byte = [0u8];
1044 loop {
1045 reader
1046 .read_exact(&mut byte)
1047 .await
1048 .map_err(|_| error::MessageReadError::Io)?;
1049 if byte[0] == MAV_STX_V2 {
1050 break;
1051 }
1052 }
1053
1054 let mut message = MAVLinkV2MessageRaw::new();
1055
1056 message.0[0] = MAV_STX_V2;
1057 reader
1058 .read_exact(message.mut_header())
1059 .await
1060 .map_err(|_| error::MessageReadError::Io)?;
1061
1062 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1063 continue;
1065 }
1066
1067 reader
1068 .read_exact(message.mut_payload_and_checksum_and_sign())
1069 .await
1070 .map_err(|_| error::MessageReadError::Io)?;
1071
1072 if message.has_valid_crc::<M>() {
1075 return Ok(message);
1076 }
1077 }
1078}
1079
1080#[inline]
1082pub fn read_v2_msg<M: Message, R: Read>(
1083 read: &mut PeekReader<R>,
1084) -> Result<(MavHeader, M), error::MessageReadError> {
1085 read_v2_msg_inner(read, None)
1086}
1087
1088#[cfg(feature = "signing")]
1090#[inline]
1091pub fn read_v2_msg_signed<M: Message, R: Read>(
1092 read: &mut PeekReader<R>,
1093 signing_data: Option<&SigningData>,
1094) -> Result<(MavHeader, M), error::MessageReadError> {
1095 read_v2_msg_inner(read, signing_data)
1096}
1097
1098fn read_v2_msg_inner<M: Message, R: Read>(
1099 read: &mut PeekReader<R>,
1100 signing_data: Option<&SigningData>,
1101) -> Result<(MavHeader, M), error::MessageReadError> {
1102 let message = read_v2_raw_message_inner::<M, _>(read, signing_data)?;
1103
1104 Ok((
1105 MavHeader {
1106 sequence: message.sequence(),
1107 system_id: message.system_id(),
1108 component_id: message.component_id(),
1109 },
1110 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1111 ))
1112}
1113
1114#[cfg(feature = "tokio-1")]
1116pub async fn read_v2_msg_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
1117 read: &mut AsyncPeekReader<R>,
1118) -> Result<(MavHeader, M), error::MessageReadError> {
1119 read_v2_msg_async_inner(read, None).await
1120}
1121
1122#[cfg(all(feature = "tokio-1", feature = "signing"))]
1124pub async fn read_v2_msg_async_signed<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
1125 read: &mut AsyncPeekReader<R>,
1126 signing_data: Option<&SigningData>,
1127) -> Result<(MavHeader, M), error::MessageReadError> {
1128 read_v2_msg_async_inner(read, signing_data).await
1129}
1130
1131#[cfg(feature = "tokio-1")]
1132async fn read_v2_msg_async_inner<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
1133 read: &mut AsyncPeekReader<R>,
1134 signing_data: Option<&SigningData>,
1135) -> Result<(MavHeader, M), error::MessageReadError> {
1136 let message = read_v2_raw_message_async_inner::<M, _>(read, signing_data).await?;
1137
1138 Ok((
1139 MavHeader {
1140 sequence: message.sequence(),
1141 system_id: message.system_id(),
1142 component_id: message.component_id(),
1143 },
1144 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1145 ))
1146}
1147
1148#[cfg(feature = "embedded")]
1153pub async fn read_v2_msg_async<M: Message, R: embedded_io_async::Read>(
1154 r: &mut R,
1155) -> Result<(MavHeader, M), error::MessageReadError> {
1156 let message = read_v2_raw_message_async::<M>(r).await?;
1157
1158 Ok((
1159 MavHeader {
1160 sequence: message.sequence(),
1161 system_id: message.system_id(),
1162 component_id: message.component_id(),
1163 },
1164 M::parse(
1165 MavlinkVersion::V2,
1166 u32::from(message.message_id()),
1167 message.payload(),
1168 )?,
1169 ))
1170}
1171
1172pub fn write_versioned_msg<M: Message, W: Write>(
1174 w: &mut W,
1175 version: MavlinkVersion,
1176 header: MavHeader,
1177 data: &M,
1178) -> Result<usize, error::MessageWriteError> {
1179 match version {
1180 MavlinkVersion::V2 => write_v2_msg(w, header, data),
1181 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1182 }
1183}
1184
1185#[cfg(feature = "signing")]
1187pub fn write_versioned_msg_signed<M: Message, W: Write>(
1188 w: &mut W,
1189 version: MavlinkVersion,
1190 header: MavHeader,
1191 data: &M,
1192 signing_data: Option<&SigningData>,
1193) -> Result<usize, error::MessageWriteError> {
1194 match version {
1195 MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data),
1196 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1197 }
1198}
1199
1200#[cfg(feature = "tokio-1")]
1202pub async fn write_versioned_msg_async<M: Message, W: tokio::io::AsyncWriteExt + Unpin>(
1203 w: &mut W,
1204 version: MavlinkVersion,
1205 header: MavHeader,
1206 data: &M,
1207) -> Result<usize, error::MessageWriteError> {
1208 match version {
1209 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1210 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1211 }
1212}
1213
1214#[cfg(all(feature = "tokio-1", feature = "signing"))]
1216pub async fn write_versioned_msg_async_signed<M: Message, W: tokio::io::AsyncWriteExt + Unpin>(
1217 w: &mut W,
1218 version: MavlinkVersion,
1219 header: MavHeader,
1220 data: &M,
1221 signing_data: Option<&SigningData>,
1222) -> Result<usize, error::MessageWriteError> {
1223 match version {
1224 MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await,
1225 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1226 }
1227}
1228
1229#[cfg(feature = "embedded")]
1234pub async fn write_versioned_msg_async<M: Message>(
1235 w: &mut impl embedded_io_async::Write,
1236 version: MavlinkVersion,
1237 header: MavHeader,
1238 data: &M,
1239) -> Result<usize, error::MessageWriteError> {
1240 match version {
1241 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1242 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1243 }
1244}
1245
1246pub fn write_v2_msg<M: Message, W: Write>(
1248 w: &mut W,
1249 header: MavHeader,
1250 data: &M,
1251) -> Result<usize, error::MessageWriteError> {
1252 let mut message_raw = MAVLinkV2MessageRaw::new();
1253 message_raw.serialize_message(header, data);
1254
1255 let payload_length: usize = message_raw.payload_length().into();
1256 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1257
1258 w.write_all(&message_raw.0[..len])?;
1259
1260 Ok(len)
1261}
1262
1263#[cfg(feature = "signing")]
1265pub fn write_v2_msg_signed<M: Message, W: Write>(
1266 w: &mut W,
1267 header: MavHeader,
1268 data: &M,
1269 signing_data: Option<&SigningData>,
1270) -> Result<usize, error::MessageWriteError> {
1271 let mut message_raw = MAVLinkV2MessageRaw::new();
1272
1273 let signature_len = if let Some(signing_data) = signing_data {
1274 if signing_data.config.sign_outgoing {
1275 message_raw.serialize_message_for_signing(header, data);
1276 signing_data.sign_message(&mut message_raw);
1277 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1278 } else {
1279 message_raw.serialize_message(header, data);
1280 0
1281 }
1282 } else {
1283 message_raw.serialize_message(header, data);
1284 0
1285 };
1286
1287 let payload_length: usize = message_raw.payload_length().into();
1288 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1289
1290 w.write_all(&message_raw.0[..len])?;
1291
1292 Ok(len)
1293}
1294
1295#[cfg(feature = "tokio-1")]
1297pub async fn write_v2_msg_async<M: Message, W: tokio::io::AsyncWriteExt + Unpin>(
1298 w: &mut W,
1299 header: MavHeader,
1300 data: &M,
1301) -> Result<usize, error::MessageWriteError> {
1302 let mut message_raw = MAVLinkV2MessageRaw::new();
1303 message_raw.serialize_message(header, data);
1304
1305 let payload_length: usize = message_raw.payload_length().into();
1306 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1307
1308 w.write_all(&message_raw.0[..len]).await?;
1309
1310 Ok(len)
1311}
1312
1313#[cfg(feature = "signing")]
1315#[cfg(feature = "tokio-1")]
1316pub async fn write_v2_msg_async_signed<M: Message, W: tokio::io::AsyncWriteExt + Unpin>(
1317 w: &mut W,
1318 header: MavHeader,
1319 data: &M,
1320 signing_data: Option<&SigningData>,
1321) -> Result<usize, error::MessageWriteError> {
1322 let mut message_raw = MAVLinkV2MessageRaw::new();
1323
1324 let signature_len = if let Some(signing_data) = signing_data {
1325 if signing_data.config.sign_outgoing {
1326 message_raw.serialize_message_for_signing(header, data);
1327 signing_data.sign_message(&mut message_raw);
1328 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1329 } else {
1330 message_raw.serialize_message(header, data);
1331 0
1332 }
1333 } else {
1334 message_raw.serialize_message(header, data);
1335 0
1336 };
1337
1338 let payload_length: usize = message_raw.payload_length().into();
1339 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1340
1341 w.write_all(&message_raw.0[..len]).await?;
1342
1343 Ok(len)
1344}
1345
1346#[cfg(feature = "embedded")]
1351pub async fn write_v2_msg_async<M: Message>(
1352 w: &mut impl embedded_io_async::Write,
1353 header: MavHeader,
1354 data: &M,
1355) -> Result<usize, error::MessageWriteError> {
1356 let mut message_raw = MAVLinkV2MessageRaw::new();
1357 message_raw.serialize_message(header, data);
1358
1359 let payload_length: usize = message_raw.payload_length().into();
1360 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1361
1362 w.write_all(&message_raw.0[..len])
1363 .await
1364 .map_err(|_| error::MessageWriteError::Io)?;
1365
1366 Ok(len)
1367}
1368
1369pub fn write_v1_msg<M: Message, W: Write>(
1371 w: &mut W,
1372 header: MavHeader,
1373 data: &M,
1374) -> Result<usize, error::MessageWriteError> {
1375 let mut message_raw = MAVLinkV1MessageRaw::new();
1376 message_raw.serialize_message(header, data);
1377
1378 let payload_length: usize = message_raw.payload_length().into();
1379 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1380
1381 w.write_all(&message_raw.0[..len])?;
1382
1383 Ok(len)
1384}
1385
1386#[cfg(feature = "tokio-1")]
1388pub async fn write_v1_msg_async<M: Message, W: tokio::io::AsyncWriteExt + Unpin>(
1389 w: &mut W,
1390 header: MavHeader,
1391 data: &M,
1392) -> Result<usize, error::MessageWriteError> {
1393 let mut message_raw = MAVLinkV1MessageRaw::new();
1394 message_raw.serialize_message(header, data);
1395
1396 let payload_length: usize = message_raw.payload_length().into();
1397 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1398
1399 w.write_all(&message_raw.0[..len]).await?;
1400
1401 Ok(len)
1402}
1403
1404#[cfg(feature = "embedded")]
1409pub async fn write_v1_msg_async<M: Message>(
1410 w: &mut impl embedded_io_async::Write,
1411 header: MavHeader,
1412 data: &M,
1413) -> Result<usize, error::MessageWriteError> {
1414 let mut message_raw = MAVLinkV1MessageRaw::new();
1415 message_raw.serialize_message(header, data);
1416
1417 let payload_length: usize = message_raw.payload_length().into();
1418 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1419
1420 w.write_all(&message_raw.0[..len])
1421 .await
1422 .map_err(|_| error::MessageWriteError::Io)?;
1423
1424 Ok(len)
1425}