1#![cfg_attr(not(feature = "std"), no_std)]
23#![cfg_attr(docsrs, feature(doc_auto_cfg))]
24#![deny(clippy::all)]
25#![warn(clippy::use_self)]
26
27use core::result::Result;
28
29#[cfg(feature = "std")]
30use std::io::{Read, Write};
31
32pub mod utils;
33#[allow(unused_imports)]
34use utils::{remove_trailing_zeroes, RustDefault};
35
36#[cfg(feature = "serde")]
37use serde::{Deserialize, Serialize};
38
39pub mod peek_reader;
40use peek_reader::PeekReader;
41
42use crate::{bytes::Bytes, error::ParserError};
43
44use crc_any::CRCu16;
45
46pub mod bytes;
47pub mod bytes_mut;
48#[cfg(feature = "std")]
49mod connection;
50pub mod error;
51#[cfg(feature = "std")]
52pub use self::connection::{connect, Connectable, MavConnection};
53
54#[cfg(feature = "tokio-1")]
55mod async_connection;
56#[cfg(feature = "tokio-1")]
57pub use self::async_connection::{connect_async, AsyncConnectable, AsyncMavConnection};
58
59#[cfg(feature = "tokio-1")]
60pub mod async_peek_reader;
61#[cfg(feature = "tokio-1")]
62use async_peek_reader::AsyncPeekReader;
63#[cfg(feature = "tokio-1")]
64use tokio::io::{AsyncWrite, AsyncWriteExt};
65
66#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
67pub mod embedded;
68#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
69use embedded::{Read, Write};
70
71#[cfg(not(feature = "signing"))]
72type SigningData = ();
73#[cfg(feature = "signing")]
74mod signing;
75#[cfg(feature = "signing")]
76pub use self::signing::{SigningConfig, SigningData};
77#[cfg(feature = "signing")]
78use sha2::{Digest, Sha256};
79
80#[cfg(feature = "arbitrary")]
81use arbitrary::Arbitrary;
82
83#[cfg(any(feature = "std", feature = "tokio-1"))]
84mod connectable;
85
86#[cfg(any(feature = "std", feature = "tokio-1"))]
87pub use connectable::ConnectionAddress;
88
89#[cfg(feature = "direct-serial")]
90pub use connection::direct_serial::config::SerialConfig;
91
92#[cfg(feature = "tcp")]
93pub use connection::tcp::config::{TcpConfig, TcpMode};
94
95#[cfg(feature = "udp")]
96pub use connection::udp::config::{UdpConfig, UdpMode};
97
98#[cfg(feature = "std")]
99pub use connection::file::config::FileConfig;
100
101pub const MAX_FRAME_SIZE: usize = 280;
105
106pub trait Message
111where
112 Self: Sized,
113{
114 fn message_id(&self) -> u32;
116
117 fn message_name(&self) -> &'static str;
119
120 fn target_system_id(&self) -> Option<u8>;
122
123 fn target_component_id(&self) -> Option<u8>;
125
126 fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize;
128
129 fn parse(
131 version: MavlinkVersion,
132 msgid: u32,
133 payload: &[u8],
134 ) -> Result<Self, error::ParserError>;
135
136 fn message_id_from_name(name: &str) -> Option<u32>;
138 fn default_message_from_id(id: u32) -> Option<Self>;
140 #[cfg(feature = "arbitrary")]
142 fn random_message_from_id<R: rand::RngCore>(id: u32, rng: &mut R) -> Option<Self>;
143 fn extra_crc(id: u32) -> u8;
145}
146
147pub trait MessageData: Sized {
148 type Message: Message;
149
150 const ID: u32;
151 const NAME: &'static str;
152 const EXTRA_CRC: u8;
153 const ENCODED_LEN: usize;
154
155 fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize;
156 fn deser(version: MavlinkVersion, payload: &[u8]) -> Result<Self, ParserError>;
157}
158
159#[derive(Debug, Copy, Clone, PartialEq, Eq)]
161#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
162#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
163pub struct MavHeader {
164 pub system_id: u8,
166 pub component_id: u8,
168 pub sequence: u8,
170}
171
172#[derive(Debug, Copy, Clone, PartialEq, Eq)]
174#[cfg_attr(feature = "serde", derive(Serialize))]
175#[cfg_attr(feature = "serde", serde(tag = "type"))]
176#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
177pub enum MavlinkVersion {
178 V1,
180 V2,
182}
183
184pub const MAV_STX: u8 = 0xFE;
186
187pub const MAV_STX_V2: u8 = 0xFD;
189
190impl Default for MavHeader {
193 fn default() -> Self {
194 Self {
195 system_id: 255,
196 component_id: 0,
197 sequence: 0,
198 }
199 }
200}
201
202#[derive(Debug, Clone)]
206#[cfg_attr(feature = "serde", derive(Serialize))]
207#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
208pub struct MavFrame<M: Message> {
209 pub header: MavHeader,
211 pub msg: M,
213 pub protocol_version: MavlinkVersion,
215}
216
217impl<M: Message> MavFrame<M> {
218 pub fn ser(&self, buf: &mut [u8]) -> usize {
222 let mut buf = bytes_mut::BytesMut::new(buf);
223
224 let mut payload_buf = [0u8; 255];
226 let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
227
228 buf.put_u8(self.header.sequence);
247 buf.put_u8(self.header.system_id);
248 buf.put_u8(self.header.component_id);
249
250 match self.protocol_version {
252 MavlinkVersion::V2 => {
253 let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
254 buf.put_slice(&bytes[..3]);
255 }
256 MavlinkVersion::V1 => {
257 buf.put_u8(self.msg.message_id() as u8); }
259 }
260
261 buf.put_slice(&payload_buf[..payload_len]);
262 buf.len()
263 }
264
265 pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result<Self, ParserError> {
269 let mut buf = Bytes::new(input);
270
271 let sequence = buf.get_u8();
281 let system_id = buf.get_u8();
282 let component_id = buf.get_u8();
283 let header = MavHeader {
284 system_id,
285 component_id,
286 sequence,
287 };
288
289 let msg_id = match version {
290 MavlinkVersion::V2 => buf.get_u24_le(),
291 MavlinkVersion::V1 => buf.get_u8().into(),
292 };
293
294 M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self {
295 header,
296 msg,
297 protocol_version: version,
298 })
299 }
300
301 pub fn header(&self) -> MavHeader {
303 self.header
304 }
305}
306
307pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 {
309 let mut crc_calculator = CRCu16::crc16mcrf4cc();
310 crc_calculator.digest(data);
311
312 crc_calculator.digest(&[extra_crc]);
313 crc_calculator.get_crc()
314}
315
316#[derive(Debug, Clone, Copy, PartialEq, Eq)]
317pub enum ReadVersion {
319 Single(MavlinkVersion),
321 Any,
323}
324
325impl ReadVersion {
326 #[cfg(feature = "std")]
327 fn from_conn_cfg<C: MavConnection<M>, M: Message>(conn: &C) -> Self {
328 if conn.allow_recv_any_version() {
329 Self::Any
330 } else {
331 conn.protocol_version().into()
332 }
333 }
334 #[cfg(feature = "tokio-1")]
335 fn from_async_conn_cfg<C: AsyncMavConnection<M>, M: Message + Sync + Send>(conn: &C) -> Self {
336 if conn.allow_recv_any_version() {
337 Self::Any
338 } else {
339 conn.protocol_version().into()
340 }
341 }
342}
343
344impl From<MavlinkVersion> for ReadVersion {
345 fn from(value: MavlinkVersion) -> Self {
346 Self::Single(value)
347 }
348}
349
350pub fn read_versioned_msg<M: Message, R: Read>(
352 r: &mut PeekReader<R>,
353 version: ReadVersion,
354) -> Result<(MavHeader, M), error::MessageReadError> {
355 match version {
356 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg(r),
357 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
358 ReadVersion::Any => read_any_msg(r),
359 }
360}
361
362pub fn read_raw_versioned_msg<M: Message, R: Read>(
364 r: &mut PeekReader<R>,
365 version: ReadVersion,
366) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
367 match version {
368 ReadVersion::Single(MavlinkVersion::V2) => {
369 Ok(MAVLinkMessageRaw::V2(read_v2_raw_message::<M, _>(r)?))
370 }
371 ReadVersion::Single(MavlinkVersion::V1) => {
372 Ok(MAVLinkMessageRaw::V1(read_v1_raw_message::<M, _>(r)?))
373 }
374 ReadVersion::Any => read_any_raw_message::<M, _>(r),
375 }
376}
377
378#[cfg(feature = "tokio-1")]
380pub async fn read_versioned_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
381 r: &mut AsyncPeekReader<R>,
382 version: ReadVersion,
383) -> Result<(MavHeader, M), error::MessageReadError> {
384 match version {
385 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async(r).await,
386 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
387 ReadVersion::Any => read_any_msg_async(r).await,
388 }
389}
390
391#[cfg(feature = "tokio-1")]
393pub async fn read_raw_versioned_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
394 r: &mut AsyncPeekReader<R>,
395 version: ReadVersion,
396) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
397 match version {
398 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
399 read_v2_raw_message_async::<M, _>(r).await?,
400 )),
401 ReadVersion::Single(MavlinkVersion::V1) => Ok(MAVLinkMessageRaw::V1(
402 read_v1_raw_message_async::<M, _>(r).await?,
403 )),
404 ReadVersion::Any => read_any_raw_message_async::<M, _>(r).await,
405 }
406}
407
408#[cfg(feature = "signing")]
413pub fn read_raw_versioned_msg_signed<M: Message, R: Read>(
414 r: &mut PeekReader<R>,
415 version: ReadVersion,
416 signing_data: Option<&SigningData>,
417) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
418 match version {
419 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
420 read_v2_raw_message_inner::<M, _>(r, signing_data)?,
421 )),
422 ReadVersion::Single(MavlinkVersion::V1) => {
423 Ok(MAVLinkMessageRaw::V1(read_v1_raw_message::<M, _>(r)?))
424 }
425 ReadVersion::Any => read_any_raw_message_inner::<M, _>(r, signing_data),
426 }
427}
428
429#[cfg(feature = "signing")]
434pub fn read_versioned_msg_signed<M: Message, R: Read>(
435 r: &mut PeekReader<R>,
436 version: ReadVersion,
437 signing_data: Option<&SigningData>,
438) -> Result<(MavHeader, M), error::MessageReadError> {
439 match version {
440 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_inner(r, signing_data),
441 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
442 ReadVersion::Any => read_any_msg_inner(r, signing_data),
443 }
444}
445
446#[cfg(all(feature = "tokio-1", feature = "signing"))]
451pub async fn read_raw_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
452 r: &mut AsyncPeekReader<R>,
453 version: ReadVersion,
454 signing_data: Option<&SigningData>,
455) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
456 match version {
457 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
458 read_v2_raw_message_async_inner::<M, _>(r, signing_data).await?,
459 )),
460 ReadVersion::Single(MavlinkVersion::V1) => Ok(MAVLinkMessageRaw::V1(
461 read_v1_raw_message_async::<M, _>(r).await?,
462 )),
463 ReadVersion::Any => read_any_raw_message_async_inner::<M, _>(r, signing_data).await,
464 }
465}
466
467#[cfg(all(feature = "tokio-1", feature = "signing"))]
472pub async fn read_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
473 r: &mut AsyncPeekReader<R>,
474 version: ReadVersion,
475 signing_data: Option<&SigningData>,
476) -> Result<(MavHeader, M), error::MessageReadError> {
477 match version {
478 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async_inner(r, signing_data).await,
479 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
480 ReadVersion::Any => read_any_msg_async_inner(r, signing_data).await,
481 }
482}
483
484#[derive(Debug, Copy, Clone, PartialEq, Eq)]
485pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]);
490
491impl Default for MAVLinkV1MessageRaw {
492 fn default() -> Self {
493 Self::new()
494 }
495}
496
497impl MAVLinkV1MessageRaw {
498 const HEADER_SIZE: usize = 5;
499
500 pub const fn new() -> Self {
502 Self([0; 1 + Self::HEADER_SIZE + 255 + 2])
503 }
504
505 pub const fn from_bytes_unparsed(bytes: [u8; 1 + Self::HEADER_SIZE + 255 + 2]) -> Self {
509 Self(bytes)
510 }
511
512 #[inline]
514 pub fn as_slice(&self) -> &[u8] {
515 &self.0
516 }
517
518 #[inline]
520 pub fn as_mut_slice(&mut self) -> &mut [u8] {
521 &mut self.0
522 }
523
524 #[inline]
526 pub fn into_inner(self) -> [u8; 1 + Self::HEADER_SIZE + 255 + 2] {
527 self.0
528 }
529
530 #[inline]
532 pub fn header(&self) -> &[u8] {
533 &self.0[1..=Self::HEADER_SIZE]
534 }
535
536 #[inline]
538 fn mut_header(&mut self) -> &mut [u8] {
539 &mut self.0[1..=Self::HEADER_SIZE]
540 }
541
542 #[inline]
544 pub fn payload_length(&self) -> u8 {
545 self.0[1]
546 }
547
548 #[inline]
550 pub fn sequence(&self) -> u8 {
551 self.0[2]
552 }
553
554 #[inline]
556 pub fn system_id(&self) -> u8 {
557 self.0[3]
558 }
559
560 #[inline]
562 pub fn component_id(&self) -> u8 {
563 self.0[4]
564 }
565
566 #[inline]
568 pub fn message_id(&self) -> u8 {
569 self.0[5]
570 }
571
572 #[inline]
574 pub fn payload(&self) -> &[u8] {
575 let payload_length: usize = self.payload_length().into();
576 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
577 }
578
579 #[inline]
581 pub fn checksum(&self) -> u16 {
582 let payload_length: usize = self.payload_length().into();
583 u16::from_le_bytes([
584 self.0[1 + Self::HEADER_SIZE + payload_length],
585 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
586 ])
587 }
588
589 #[inline]
590 fn mut_payload_and_checksum(&mut self) -> &mut [u8] {
591 let payload_length: usize = self.payload_length().into();
592 &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)]
593 }
594
595 #[inline]
597 pub fn has_valid_crc<M: Message>(&self) -> bool {
598 let payload_length: usize = self.payload_length().into();
599 self.checksum()
600 == calculate_crc(
601 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
602 M::extra_crc(self.message_id().into()),
603 )
604 }
605
606 pub fn raw_bytes(&self) -> &[u8] {
608 let payload_length = self.payload_length() as usize;
609 &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)]
610 }
611
612 fn serialize_stx_and_header_and_crc(
613 &mut self,
614 header: MavHeader,
615 msgid: u32,
616 payload_length: usize,
617 extra_crc: u8,
618 ) {
619 self.0[0] = MAV_STX;
620
621 let header_buf = self.mut_header();
622 header_buf.copy_from_slice(&[
623 payload_length as u8,
624 header.sequence,
625 header.system_id,
626 header.component_id,
627 msgid as u8,
628 ]);
629
630 let crc = calculate_crc(
631 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
632 extra_crc,
633 );
634 self.0[(1 + Self::HEADER_SIZE + payload_length)
635 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
636 .copy_from_slice(&crc.to_le_bytes());
637 }
638
639 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
641 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
642 let payload_length = message.ser(MavlinkVersion::V1, payload_buf);
643
644 let message_id = message.message_id();
645 self.serialize_stx_and_header_and_crc(
646 header,
647 message_id,
648 payload_length,
649 M::extra_crc(message_id),
650 );
651 }
652
653 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
654 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
655 let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf);
656
657 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC);
658 }
659}
660
661fn try_decode_v1<M: Message, R: Read>(
662 reader: &mut PeekReader<R>,
663) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
664 let mut message = MAVLinkV1MessageRaw::new();
665 let whole_header_size = MAVLinkV1MessageRaw::HEADER_SIZE + 1;
666
667 message.0[0] = MAV_STX;
668 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
669 message.mut_header().copy_from_slice(header);
670 let packet_length = message.raw_bytes().len();
671 let payload_and_checksum = &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
672 message
673 .mut_payload_and_checksum()
674 .copy_from_slice(payload_and_checksum);
675
676 if message.has_valid_crc::<M>() {
679 reader.consume(message.raw_bytes().len());
680 Ok(Some(message))
681 } else {
682 Ok(None)
683 }
684}
685
686#[cfg(feature = "tokio-1")]
687async fn try_decode_v1_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
689 reader: &mut AsyncPeekReader<R>,
690) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
691 let mut message = MAVLinkV1MessageRaw::new();
692
693 message.0[0] = MAV_STX;
694 let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await?
695 [..MAVLinkV1MessageRaw::HEADER_SIZE];
696 message.mut_header().copy_from_slice(header);
697 let packet_length = message.raw_bytes().len() - 1;
698 let payload_and_checksum =
699 &reader.peek_exact(packet_length).await?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length];
700 message
701 .mut_payload_and_checksum()
702 .copy_from_slice(payload_and_checksum);
703
704 if message.has_valid_crc::<M>() {
707 reader.consume(message.raw_bytes().len() - 1);
708 Ok(Some(message))
709 } else {
710 Ok(None)
711 }
712}
713
714pub fn read_v1_raw_message<M: Message, R: Read>(
716 reader: &mut PeekReader<R>,
717) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
718 loop {
719 while reader.peek_exact(1)?[0] != MAV_STX {
721 reader.consume(1);
722 }
723
724 if let Some(msg) = try_decode_v1::<M, _>(reader)? {
725 return Ok(msg);
726 };
727
728 reader.consume(1);
729 }
730}
731
732#[cfg(feature = "tokio-1")]
734pub async fn read_v1_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
735 reader: &mut AsyncPeekReader<R>,
736) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
737 loop {
738 loop {
739 if reader.read_u8().await? == MAV_STX {
741 break;
742 }
743 }
744
745 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
746 return Ok(message);
747 }
748 }
749}
750
751#[cfg(feature = "embedded")]
757pub async fn read_v1_raw_message_async<M: Message>(
758 reader: &mut impl embedded_io_async::Read,
759) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
760 loop {
761 let mut byte = [0u8];
763 loop {
764 reader
765 .read_exact(&mut byte)
766 .await
767 .map_err(|_| error::MessageReadError::Io)?;
768 if byte[0] == MAV_STX {
769 break;
770 }
771 }
772
773 let mut message = MAVLinkV1MessageRaw::new();
774
775 message.0[0] = MAV_STX;
776 reader
777 .read_exact(message.mut_header())
778 .await
779 .map_err(|_| error::MessageReadError::Io)?;
780 reader
781 .read_exact(message.mut_payload_and_checksum())
782 .await
783 .map_err(|_| error::MessageReadError::Io)?;
784
785 if message.has_valid_crc::<M>() {
788 return Ok(message);
789 }
790 }
791}
792
793pub fn read_v1_msg<M: Message, R: Read>(
795 r: &mut PeekReader<R>,
796) -> Result<(MavHeader, M), error::MessageReadError> {
797 let message = read_v1_raw_message::<M, _>(r)?;
798
799 Ok((
800 MavHeader {
801 sequence: message.sequence(),
802 system_id: message.system_id(),
803 component_id: message.component_id(),
804 },
805 M::parse(
806 MavlinkVersion::V1,
807 u32::from(message.message_id()),
808 message.payload(),
809 )?,
810 ))
811}
812
813#[cfg(feature = "tokio-1")]
815pub async fn read_v1_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
816 r: &mut AsyncPeekReader<R>,
817) -> Result<(MavHeader, M), error::MessageReadError> {
818 let message = read_v1_raw_message_async::<M, _>(r).await?;
819
820 Ok((
821 MavHeader {
822 sequence: message.sequence(),
823 system_id: message.system_id(),
824 component_id: message.component_id(),
825 },
826 M::parse(
827 MavlinkVersion::V1,
828 u32::from(message.message_id()),
829 message.payload(),
830 )?,
831 ))
832}
833
834#[cfg(feature = "embedded")]
839pub async fn read_v1_msg_async<M: Message>(
840 r: &mut impl embedded_io_async::Read,
841) -> Result<(MavHeader, M), error::MessageReadError> {
842 let message = read_v1_raw_message_async::<M>(r).await?;
843
844 Ok((
845 MavHeader {
846 sequence: message.sequence(),
847 system_id: message.system_id(),
848 component_id: message.component_id(),
849 },
850 M::parse(
851 MavlinkVersion::V1,
852 u32::from(message.message_id()),
853 message.payload(),
854 )?,
855 ))
856}
857
858const MAVLINK_IFLAG_SIGNED: u8 = 0x01;
859const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED;
860
861#[derive(Debug, Copy, Clone, PartialEq, Eq)]
862pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]);
867
868impl Default for MAVLinkV2MessageRaw {
869 fn default() -> Self {
870 Self::new()
871 }
872}
873
874impl MAVLinkV2MessageRaw {
875 const HEADER_SIZE: usize = 9;
876 const SIGNATURE_SIZE: usize = 13;
877
878 pub const fn new() -> Self {
880 Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE])
881 }
882
883 pub const fn from_bytes_unparsed(
887 bytes: [u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE],
888 ) -> Self {
889 Self(bytes)
890 }
891
892 #[inline]
894 pub fn as_slice(&self) -> &[u8] {
895 &self.0
896 }
897
898 #[inline]
900 pub fn as_mut_slice(&mut self) -> &mut [u8] {
901 &mut self.0
902 }
903
904 #[inline]
906 pub fn into_inner(self) -> [u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE] {
907 self.0
908 }
909
910 #[inline]
912 pub fn header(&self) -> &[u8] {
913 &self.0[1..=Self::HEADER_SIZE]
914 }
915
916 #[inline]
918 fn mut_header(&mut self) -> &mut [u8] {
919 &mut self.0[1..=Self::HEADER_SIZE]
920 }
921
922 #[inline]
924 pub fn payload_length(&self) -> u8 {
925 self.0[1]
926 }
927
928 #[inline]
932 pub fn incompatibility_flags(&self) -> u8 {
933 self.0[2]
934 }
935
936 #[inline]
940 pub fn incompatibility_flags_mut(&mut self) -> &mut u8 {
941 &mut self.0[2]
942 }
943
944 #[inline]
946 pub fn compatibility_flags(&self) -> u8 {
947 self.0[3]
948 }
949
950 #[inline]
952 pub fn sequence(&self) -> u8 {
953 self.0[4]
954 }
955
956 #[inline]
958 pub fn system_id(&self) -> u8 {
959 self.0[5]
960 }
961
962 #[inline]
964 pub fn component_id(&self) -> u8 {
965 self.0[6]
966 }
967
968 #[inline]
970 pub fn message_id(&self) -> u32 {
971 u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0])
972 }
973
974 #[inline]
976 pub fn payload(&self) -> &[u8] {
977 let payload_length: usize = self.payload_length().into();
978 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
979 }
980
981 #[inline]
983 pub fn checksum(&self) -> u16 {
984 let payload_length: usize = self.payload_length().into();
985 u16::from_le_bytes([
986 self.0[1 + Self::HEADER_SIZE + payload_length],
987 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
988 ])
989 }
990
991 #[cfg(feature = "signing")]
993 #[inline]
994 pub fn checksum_bytes(&self) -> &[u8] {
995 let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize;
996 &self.0[checksum_offset..(checksum_offset + 2)]
997 }
998
999 #[cfg(feature = "signing")]
1003 #[inline]
1004 pub fn signature_link_id(&self) -> u8 {
1005 let payload_length: usize = self.payload_length().into();
1006 self.0[1 + Self::HEADER_SIZE + payload_length + 2]
1007 }
1008
1009 #[cfg(feature = "signing")]
1011 #[inline]
1012 pub fn signature_link_id_mut(&mut self) -> &mut u8 {
1013 let payload_length: usize = self.payload_length().into();
1014 &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2]
1015 }
1016
1017 #[cfg(feature = "signing")]
1023 #[inline]
1024 pub fn signature_timestamp(&self) -> u64 {
1025 let mut timestamp_bytes = [0u8; 8];
1026 timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes());
1027 u64::from_le_bytes(timestamp_bytes)
1028 }
1029
1030 #[cfg(feature = "signing")]
1034 #[inline]
1035 pub fn signature_timestamp_bytes(&self) -> &[u8] {
1036 let payload_length: usize = self.payload_length().into();
1037 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
1038 &self.0[timestamp_start..(timestamp_start + 6)]
1039 }
1040
1041 #[cfg(feature = "signing")]
1043 #[inline]
1044 pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] {
1045 let payload_length: usize = self.payload_length().into();
1046 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
1047 &mut self.0[timestamp_start..(timestamp_start + 6)]
1048 }
1049
1050 #[cfg(feature = "signing")]
1054 #[inline]
1055 pub fn signature_value(&self) -> &[u8] {
1056 let payload_length: usize = self.payload_length().into();
1057 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
1058 &self.0[signature_start..(signature_start + 6)]
1059 }
1060
1061 #[cfg(feature = "signing")]
1063 #[inline]
1064 pub fn signature_value_mut(&mut self) -> &mut [u8] {
1065 let payload_length: usize = self.payload_length().into();
1066 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
1067 &mut self.0[signature_start..(signature_start + 6)]
1068 }
1069
1070 fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] {
1071 let payload_length: usize = self.payload_length().into();
1072
1073 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
1075 0
1076 } else {
1077 Self::SIGNATURE_SIZE
1078 };
1079
1080 &mut self.0
1081 [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
1082 }
1083
1084 #[inline]
1086 pub fn has_valid_crc<M: Message>(&self) -> bool {
1087 let payload_length: usize = self.payload_length().into();
1088 self.checksum()
1089 == calculate_crc(
1090 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
1091 M::extra_crc(self.message_id()),
1092 )
1093 }
1094
1095 #[cfg(feature = "signing")]
1099 pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) {
1100 let mut hasher = Sha256::new();
1101 hasher.update(secret_key);
1102 hasher.update([MAV_STX_V2]);
1103 hasher.update(self.header());
1104 hasher.update(self.payload());
1105 hasher.update(self.checksum_bytes());
1106 hasher.update([self.signature_link_id()]);
1107 hasher.update(self.signature_timestamp_bytes());
1108 target_buffer.copy_from_slice(&hasher.finalize()[0..6]);
1109 }
1110
1111 pub fn raw_bytes(&self) -> &[u8] {
1113 let payload_length = self.payload_length() as usize;
1114
1115 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
1116 0
1117 } else {
1118 Self::SIGNATURE_SIZE
1119 };
1120
1121 &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
1122 }
1123
1124 fn serialize_stx_and_header_and_crc(
1125 &mut self,
1126 header: MavHeader,
1127 msgid: u32,
1128 payload_length: usize,
1129 extra_crc: u8,
1130 incompat_flags: u8,
1131 ) {
1132 self.0[0] = MAV_STX_V2;
1133 let msgid_bytes = msgid.to_le_bytes();
1134
1135 let header_buf = self.mut_header();
1136 header_buf.copy_from_slice(&[
1137 payload_length as u8,
1138 incompat_flags,
1139 0, header.sequence,
1141 header.system_id,
1142 header.component_id,
1143 msgid_bytes[0],
1144 msgid_bytes[1],
1145 msgid_bytes[2],
1146 ]);
1147
1148 let crc = calculate_crc(
1149 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
1150 extra_crc,
1151 );
1152 self.0[(1 + Self::HEADER_SIZE + payload_length)
1153 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
1154 .copy_from_slice(&crc.to_le_bytes());
1155 }
1156
1157 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
1161 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1162 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1163
1164 let message_id = message.message_id();
1165 self.serialize_stx_and_header_and_crc(
1166 header,
1167 message_id,
1168 payload_length,
1169 M::extra_crc(message_id),
1170 0,
1171 );
1172 }
1173
1174 pub fn serialize_message_for_signing<M: Message>(&mut self, header: MavHeader, message: &M) {
1179 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1180 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1181
1182 let message_id = message.message_id();
1183 self.serialize_stx_and_header_and_crc(
1184 header,
1185 message_id,
1186 payload_length,
1187 M::extra_crc(message_id),
1188 MAVLINK_IFLAG_SIGNED,
1189 );
1190 }
1191
1192 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
1193 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1194 let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf);
1195
1196 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0);
1197 }
1198}
1199
1200#[allow(unused_variables)]
1201fn try_decode_v2<M: Message, R: Read>(
1202 reader: &mut PeekReader<R>,
1203 signing_data: Option<&SigningData>,
1204) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1205 let mut message = MAVLinkV2MessageRaw::new();
1206 let whole_header_size = MAVLinkV2MessageRaw::HEADER_SIZE + 1;
1207
1208 message.0[0] = MAV_STX_V2;
1209 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
1210 message.mut_header().copy_from_slice(header);
1211
1212 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1213 reader.consume(1);
1215 return Ok(None);
1216 }
1217
1218 let packet_length = message.raw_bytes().len();
1219 let payload_and_checksum_and_sign =
1220 &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
1221 message
1222 .mut_payload_and_checksum_and_sign()
1223 .copy_from_slice(payload_and_checksum_and_sign);
1224
1225 if message.has_valid_crc::<M>() {
1226 reader.consume(message.raw_bytes().len());
1228 } else {
1229 reader.consume(1);
1230 return Ok(None);
1231 }
1232
1233 #[cfg(feature = "signing")]
1234 if let Some(signing_data) = signing_data {
1235 if !signing_data.verify_signature(&message) {
1236 return Ok(None);
1237 }
1238 }
1239
1240 Ok(Some(message))
1241}
1242
1243#[cfg(feature = "tokio-1")]
1244#[allow(unused_variables)]
1245async fn try_decode_v2_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1247 reader: &mut AsyncPeekReader<R>,
1248 signing_data: Option<&SigningData>,
1249) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1250 let mut message = MAVLinkV2MessageRaw::new();
1251
1252 message.0[0] = MAV_STX_V2;
1253 let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE).await?
1254 [..MAVLinkV2MessageRaw::HEADER_SIZE];
1255 message.mut_header().copy_from_slice(header);
1256
1257 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1258 return Ok(None);
1260 }
1261
1262 let packet_length = message.raw_bytes().len() - 1;
1263 let payload_and_checksum_and_sign =
1264 &reader.peek_exact(packet_length).await?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length];
1265 message
1266 .mut_payload_and_checksum_and_sign()
1267 .copy_from_slice(payload_and_checksum_and_sign);
1268
1269 if message.has_valid_crc::<M>() {
1270 reader.consume(message.raw_bytes().len() - 1);
1272 } else {
1273 return Ok(None);
1274 }
1275
1276 #[cfg(feature = "signing")]
1277 if let Some(signing_data) = signing_data {
1278 if !signing_data.verify_signature(&message) {
1279 return Ok(None);
1280 }
1281 }
1282
1283 Ok(Some(message))
1284}
1285
1286#[inline]
1288pub fn read_v2_raw_message<M: Message, R: Read>(
1289 reader: &mut PeekReader<R>,
1290) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1291 read_v2_raw_message_inner::<M, R>(reader, None)
1292}
1293
1294#[cfg(feature = "signing")]
1296#[inline]
1297pub fn read_v2_raw_message_signed<M: Message, R: Read>(
1298 reader: &mut PeekReader<R>,
1299 signing_data: Option<&SigningData>,
1300) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1301 read_v2_raw_message_inner::<M, R>(reader, signing_data)
1302}
1303
1304#[allow(unused_variables)]
1305fn read_v2_raw_message_inner<M: Message, R: Read>(
1306 reader: &mut PeekReader<R>,
1307 signing_data: Option<&SigningData>,
1308) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1309 loop {
1310 while reader.peek_exact(1)?[0] != MAV_STX_V2 {
1312 reader.consume(1);
1313 }
1314
1315 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1316 return Ok(message);
1317 }
1318 }
1319}
1320
1321#[cfg(feature = "tokio-1")]
1323pub async fn read_v2_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1324 reader: &mut AsyncPeekReader<R>,
1325) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1326 read_v2_raw_message_async_inner::<M, R>(reader, None).await
1327}
1328
1329#[cfg(feature = "tokio-1")]
1330#[allow(unused_variables)]
1331async fn read_v2_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1332 reader: &mut AsyncPeekReader<R>,
1333 signing_data: Option<&SigningData>,
1334) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1335 loop {
1336 loop {
1337 if reader.read_u8().await? == MAV_STX_V2 {
1339 break;
1340 }
1341 }
1342
1343 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1344 return Ok(message);
1345 }
1346 }
1347}
1348
1349#[cfg(all(feature = "tokio-1", feature = "signing"))]
1351pub async fn read_v2_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1352 reader: &mut AsyncPeekReader<R>,
1353 signing_data: Option<&SigningData>,
1354) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1355 read_v2_raw_message_async_inner::<M, R>(reader, signing_data).await
1356}
1357
1358#[cfg(feature = "embedded")]
1363pub async fn read_v2_raw_message_async<M: Message>(
1364 reader: &mut impl embedded_io_async::Read,
1365) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1366 loop {
1367 let mut byte = [0u8];
1369 loop {
1370 reader
1371 .read_exact(&mut byte)
1372 .await
1373 .map_err(|_| error::MessageReadError::Io)?;
1374 if byte[0] == MAV_STX_V2 {
1375 break;
1376 }
1377 }
1378
1379 let mut message = MAVLinkV2MessageRaw::new();
1380
1381 message.0[0] = MAV_STX_V2;
1382 reader
1383 .read_exact(message.mut_header())
1384 .await
1385 .map_err(|_| error::MessageReadError::Io)?;
1386
1387 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1388 continue;
1390 }
1391
1392 reader
1393 .read_exact(message.mut_payload_and_checksum_and_sign())
1394 .await
1395 .map_err(|_| error::MessageReadError::Io)?;
1396
1397 if message.has_valid_crc::<M>() {
1400 return Ok(message);
1401 }
1402 }
1403}
1404
1405#[inline]
1407pub fn read_v2_msg<M: Message, R: Read>(
1408 read: &mut PeekReader<R>,
1409) -> Result<(MavHeader, M), error::MessageReadError> {
1410 read_v2_msg_inner(read, None)
1411}
1412
1413#[cfg(feature = "signing")]
1415#[inline]
1416pub fn read_v2_msg_signed<M: Message, R: Read>(
1417 read: &mut PeekReader<R>,
1418 signing_data: Option<&SigningData>,
1419) -> Result<(MavHeader, M), error::MessageReadError> {
1420 read_v2_msg_inner(read, signing_data)
1421}
1422
1423fn read_v2_msg_inner<M: Message, R: Read>(
1424 read: &mut PeekReader<R>,
1425 signing_data: Option<&SigningData>,
1426) -> Result<(MavHeader, M), error::MessageReadError> {
1427 let message = read_v2_raw_message_inner::<M, _>(read, signing_data)?;
1428
1429 Ok((
1430 MavHeader {
1431 sequence: message.sequence(),
1432 system_id: message.system_id(),
1433 component_id: message.component_id(),
1434 },
1435 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1436 ))
1437}
1438
1439#[cfg(feature = "tokio-1")]
1441pub async fn read_v2_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1442 read: &mut AsyncPeekReader<R>,
1443) -> Result<(MavHeader, M), error::MessageReadError> {
1444 read_v2_msg_async_inner(read, None).await
1445}
1446
1447#[cfg(all(feature = "tokio-1", feature = "signing"))]
1449pub async fn read_v2_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1450 read: &mut AsyncPeekReader<R>,
1451 signing_data: Option<&SigningData>,
1452) -> Result<(MavHeader, M), error::MessageReadError> {
1453 read_v2_msg_async_inner(read, signing_data).await
1454}
1455
1456#[cfg(feature = "tokio-1")]
1457async fn read_v2_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1458 read: &mut AsyncPeekReader<R>,
1459 signing_data: Option<&SigningData>,
1460) -> Result<(MavHeader, M), error::MessageReadError> {
1461 let message = read_v2_raw_message_async_inner::<M, _>(read, signing_data).await?;
1462
1463 Ok((
1464 MavHeader {
1465 sequence: message.sequence(),
1466 system_id: message.system_id(),
1467 component_id: message.component_id(),
1468 },
1469 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1470 ))
1471}
1472
1473#[cfg(feature = "embedded")]
1478pub async fn read_v2_msg_async<M: Message, R: embedded_io_async::Read>(
1479 r: &mut R,
1480) -> Result<(MavHeader, M), error::MessageReadError> {
1481 let message = read_v2_raw_message_async::<M>(r).await?;
1482
1483 Ok((
1484 MavHeader {
1485 sequence: message.sequence(),
1486 system_id: message.system_id(),
1487 component_id: message.component_id(),
1488 },
1489 M::parse(
1490 MavlinkVersion::V2,
1491 u32::from(message.message_id()),
1492 message.payload(),
1493 )?,
1494 ))
1495}
1496
1497pub enum MAVLinkMessageRaw {
1499 V1(MAVLinkV1MessageRaw),
1500 V2(MAVLinkV2MessageRaw),
1501}
1502
1503impl MAVLinkMessageRaw {
1504 pub fn payload(&self) -> &[u8] {
1505 match self {
1506 Self::V1(msg) => msg.payload(),
1507 Self::V2(msg) => msg.payload(),
1508 }
1509 }
1510 pub fn sequence(&self) -> u8 {
1511 match self {
1512 Self::V1(msg) => msg.sequence(),
1513 Self::V2(msg) => msg.sequence(),
1514 }
1515 }
1516 pub fn system_id(&self) -> u8 {
1517 match self {
1518 Self::V1(msg) => msg.system_id(),
1519 Self::V2(msg) => msg.system_id(),
1520 }
1521 }
1522 pub fn component_id(&self) -> u8 {
1523 match self {
1524 Self::V1(msg) => msg.component_id(),
1525 Self::V2(msg) => msg.component_id(),
1526 }
1527 }
1528 pub fn message_id(&self) -> u32 {
1529 match self {
1530 Self::V1(msg) => u32::from(msg.message_id()),
1531 Self::V2(msg) => msg.message_id(),
1532 }
1533 }
1534 pub fn version(&self) -> MavlinkVersion {
1535 match self {
1536 Self::V1(_) => MavlinkVersion::V1,
1537 Self::V2(_) => MavlinkVersion::V2,
1538 }
1539 }
1540}
1541
1542#[inline]
1544pub fn read_any_raw_message<M: Message, R: Read>(
1545 reader: &mut PeekReader<R>,
1546) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1547 read_any_raw_message_inner::<M, R>(reader, None)
1548}
1549
1550#[cfg(feature = "signing")]
1552#[inline]
1553pub fn read_any_raw_message_signed<M: Message, R: Read>(
1554 reader: &mut PeekReader<R>,
1555 signing_data: Option<&SigningData>,
1556) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1557 read_any_raw_message_inner::<M, R>(reader, signing_data)
1558}
1559
1560#[allow(unused_variables)]
1561fn read_any_raw_message_inner<M: Message, R: Read>(
1562 reader: &mut PeekReader<R>,
1563 signing_data: Option<&SigningData>,
1564) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1565 loop {
1566 let version = loop {
1568 let byte = reader.peek_exact(1)?[0];
1569 if byte == MAV_STX {
1570 break MavlinkVersion::V1;
1571 }
1572 if byte == MAV_STX_V2 {
1573 break MavlinkVersion::V2;
1574 }
1575 reader.consume(1);
1576 };
1577 match version {
1578 MavlinkVersion::V1 => {
1579 if let Some(message) = try_decode_v1::<M, _>(reader)? {
1580 #[cfg(feature = "signing")]
1582 if let Some(signing) = signing_data {
1583 if signing.config.allow_unsigned {
1584 return Ok(MAVLinkMessageRaw::V1(message));
1585 }
1586 } else {
1587 return Ok(MAVLinkMessageRaw::V1(message));
1588 }
1589 #[cfg(not(feature = "signing"))]
1590 return Ok(MAVLinkMessageRaw::V1(message));
1591 }
1592
1593 reader.consume(1);
1594 }
1595 MavlinkVersion::V2 => {
1596 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1597 return Ok(MAVLinkMessageRaw::V2(message));
1598 }
1599 }
1600 }
1601 }
1602}
1603
1604#[cfg(feature = "tokio-1")]
1606pub async fn read_any_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1607 reader: &mut AsyncPeekReader<R>,
1608) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1609 read_any_raw_message_async_inner::<M, R>(reader, None).await
1610}
1611
1612#[cfg(all(feature = "tokio-1", feature = "signing"))]
1614pub async fn read_any_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1615 reader: &mut AsyncPeekReader<R>,
1616 signing_data: Option<&SigningData>,
1617) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1618 read_any_raw_message_async_inner::<M, R>(reader, signing_data).await
1619}
1620
1621#[cfg(feature = "tokio-1")]
1622#[allow(unused_variables)]
1623async fn read_any_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1624 reader: &mut AsyncPeekReader<R>,
1625 signing_data: Option<&SigningData>,
1626) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1627 loop {
1628 let version = loop {
1630 let read = reader.read_u8().await?;
1631 if read == MAV_STX {
1632 break MavlinkVersion::V1;
1633 }
1634 if read == MAV_STX_V2 {
1635 break MavlinkVersion::V2;
1636 }
1637 };
1638
1639 match version {
1640 MavlinkVersion::V1 => {
1641 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
1642 #[cfg(feature = "signing")]
1644 if let Some(signing) = signing_data {
1645 if signing.config.allow_unsigned {
1646 return Ok(MAVLinkMessageRaw::V1(message));
1647 }
1648 } else {
1649 return Ok(MAVLinkMessageRaw::V1(message));
1650 }
1651 #[cfg(not(feature = "signing"))]
1652 return Ok(MAVLinkMessageRaw::V1(message));
1653 }
1654 }
1655 MavlinkVersion::V2 => {
1656 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1657 return Ok(MAVLinkMessageRaw::V2(message));
1658 }
1659 }
1660 }
1661 }
1662}
1663
1664#[inline]
1666pub fn read_any_msg<M: Message, R: Read>(
1667 read: &mut PeekReader<R>,
1668) -> Result<(MavHeader, M), error::MessageReadError> {
1669 read_any_msg_inner(read, None)
1670}
1671
1672#[cfg(feature = "signing")]
1676#[inline]
1677pub fn read_any_msg_signed<M: Message, R: Read>(
1678 read: &mut PeekReader<R>,
1679 signing_data: Option<&SigningData>,
1680) -> Result<(MavHeader, M), error::MessageReadError> {
1681 read_any_msg_inner(read, signing_data)
1682}
1683
1684fn read_any_msg_inner<M: Message, R: Read>(
1685 read: &mut PeekReader<R>,
1686 signing_data: Option<&SigningData>,
1687) -> Result<(MavHeader, M), error::MessageReadError> {
1688 let message = read_any_raw_message_inner::<M, _>(read, signing_data)?;
1689 Ok((
1690 MavHeader {
1691 sequence: message.sequence(),
1692 system_id: message.system_id(),
1693 component_id: message.component_id(),
1694 },
1695 M::parse(message.version(), message.message_id(), message.payload())?,
1696 ))
1697}
1698
1699#[cfg(feature = "tokio-1")]
1701pub async fn read_any_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1702 read: &mut AsyncPeekReader<R>,
1703) -> Result<(MavHeader, M), error::MessageReadError> {
1704 read_any_msg_async_inner(read, None).await
1705}
1706
1707#[cfg(all(feature = "tokio-1", feature = "signing"))]
1711#[inline]
1712pub async fn read_any_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1713 read: &mut AsyncPeekReader<R>,
1714 signing_data: Option<&SigningData>,
1715) -> Result<(MavHeader, M), error::MessageReadError> {
1716 read_any_msg_async_inner(read, signing_data).await
1717}
1718
1719#[cfg(feature = "tokio-1")]
1720async fn read_any_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1721 read: &mut AsyncPeekReader<R>,
1722 signing_data: Option<&SigningData>,
1723) -> Result<(MavHeader, M), error::MessageReadError> {
1724 let message = read_any_raw_message_async_inner::<M, _>(read, signing_data).await?;
1725
1726 Ok((
1727 MavHeader {
1728 sequence: message.sequence(),
1729 system_id: message.system_id(),
1730 component_id: message.component_id(),
1731 },
1732 M::parse(message.version(), message.message_id(), message.payload())?,
1733 ))
1734}
1735
1736pub fn write_versioned_msg<M: Message, W: Write>(
1738 w: &mut W,
1739 version: MavlinkVersion,
1740 header: MavHeader,
1741 data: &M,
1742) -> Result<usize, error::MessageWriteError> {
1743 match version {
1744 MavlinkVersion::V2 => write_v2_msg(w, header, data),
1745 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1746 }
1747}
1748
1749#[cfg(feature = "signing")]
1753pub fn write_versioned_msg_signed<M: Message, W: Write>(
1754 w: &mut W,
1755 version: MavlinkVersion,
1756 header: MavHeader,
1757 data: &M,
1758 signing_data: Option<&SigningData>,
1759) -> Result<usize, error::MessageWriteError> {
1760 match version {
1761 MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data),
1762 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1763 }
1764}
1765
1766#[cfg(feature = "tokio-1")]
1768pub async fn write_versioned_msg_async<M: Message, W: AsyncWrite + Unpin>(
1769 w: &mut W,
1770 version: MavlinkVersion,
1771 header: MavHeader,
1772 data: &M,
1773) -> Result<usize, error::MessageWriteError> {
1774 match version {
1775 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1776 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1777 }
1778}
1779
1780#[cfg(all(feature = "tokio-1", feature = "signing"))]
1784pub async fn write_versioned_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1785 w: &mut W,
1786 version: MavlinkVersion,
1787 header: MavHeader,
1788 data: &M,
1789 signing_data: Option<&SigningData>,
1790) -> Result<usize, error::MessageWriteError> {
1791 match version {
1792 MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await,
1793 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1794 }
1795}
1796
1797#[cfg(feature = "embedded")]
1802pub async fn write_versioned_msg_async<M: Message>(
1803 w: &mut impl embedded_io_async::Write,
1804 version: MavlinkVersion,
1805 header: MavHeader,
1806 data: &M,
1807) -> Result<usize, error::MessageWriteError> {
1808 match version {
1809 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1810 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1811 }
1812}
1813
1814pub fn write_v2_msg<M: Message, W: Write>(
1816 w: &mut W,
1817 header: MavHeader,
1818 data: &M,
1819) -> Result<usize, error::MessageWriteError> {
1820 let mut message_raw = MAVLinkV2MessageRaw::new();
1821 message_raw.serialize_message(header, data);
1822
1823 let payload_length: usize = message_raw.payload_length().into();
1824 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1825
1826 w.write_all(&message_raw.0[..len])?;
1827
1828 Ok(len)
1829}
1830
1831#[cfg(feature = "signing")]
1833pub fn write_v2_msg_signed<M: Message, W: Write>(
1834 w: &mut W,
1835 header: MavHeader,
1836 data: &M,
1837 signing_data: Option<&SigningData>,
1838) -> Result<usize, error::MessageWriteError> {
1839 let mut message_raw = MAVLinkV2MessageRaw::new();
1840
1841 let signature_len = if let Some(signing_data) = signing_data {
1842 if signing_data.config.sign_outgoing {
1843 message_raw.serialize_message_for_signing(header, data);
1844 signing_data.sign_message(&mut message_raw);
1845 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1846 } else {
1847 message_raw.serialize_message(header, data);
1848 0
1849 }
1850 } else {
1851 message_raw.serialize_message(header, data);
1852 0
1853 };
1854
1855 let payload_length: usize = message_raw.payload_length().into();
1856 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1857
1858 w.write_all(&message_raw.0[..len])?;
1859
1860 Ok(len)
1861}
1862
1863#[cfg(feature = "tokio-1")]
1865pub async fn write_v2_msg_async<M: Message, W: AsyncWrite + Unpin>(
1866 w: &mut W,
1867 header: MavHeader,
1868 data: &M,
1869) -> Result<usize, error::MessageWriteError> {
1870 let mut message_raw = MAVLinkV2MessageRaw::new();
1871 message_raw.serialize_message(header, data);
1872
1873 let payload_length: usize = message_raw.payload_length().into();
1874 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1875
1876 w.write_all(&message_raw.0[..len]).await?;
1877
1878 Ok(len)
1879}
1880
1881#[cfg(feature = "signing")]
1883#[cfg(feature = "tokio-1")]
1884pub async fn write_v2_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1885 w: &mut W,
1886 header: MavHeader,
1887 data: &M,
1888 signing_data: Option<&SigningData>,
1889) -> Result<usize, error::MessageWriteError> {
1890 let mut message_raw = MAVLinkV2MessageRaw::new();
1891
1892 let signature_len = if let Some(signing_data) = signing_data {
1893 if signing_data.config.sign_outgoing {
1894 message_raw.serialize_message_for_signing(header, data);
1895 signing_data.sign_message(&mut message_raw);
1896 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1897 } else {
1898 message_raw.serialize_message(header, data);
1899 0
1900 }
1901 } else {
1902 message_raw.serialize_message(header, data);
1903 0
1904 };
1905
1906 let payload_length: usize = message_raw.payload_length().into();
1907 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1908
1909 w.write_all(&message_raw.0[..len]).await?;
1910
1911 Ok(len)
1912}
1913
1914#[cfg(feature = "embedded")]
1919pub async fn write_v2_msg_async<M: Message>(
1920 w: &mut impl embedded_io_async::Write,
1921 header: MavHeader,
1922 data: &M,
1923) -> Result<usize, error::MessageWriteError> {
1924 let mut message_raw = MAVLinkV2MessageRaw::new();
1925 message_raw.serialize_message(header, data);
1926
1927 let payload_length: usize = message_raw.payload_length().into();
1928 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1929
1930 w.write_all(&message_raw.0[..len])
1931 .await
1932 .map_err(|_| error::MessageWriteError::Io)?;
1933
1934 Ok(len)
1935}
1936
1937pub fn write_v1_msg<M: Message, W: Write>(
1939 w: &mut W,
1940 header: MavHeader,
1941 data: &M,
1942) -> Result<usize, error::MessageWriteError> {
1943 let mut message_raw = MAVLinkV1MessageRaw::new();
1944 message_raw.serialize_message(header, data);
1945
1946 let payload_length: usize = message_raw.payload_length().into();
1947 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1948
1949 w.write_all(&message_raw.0[..len])?;
1950
1951 Ok(len)
1952}
1953
1954#[cfg(feature = "tokio-1")]
1956pub async fn write_v1_msg_async<M: Message, W: AsyncWrite + Unpin>(
1957 w: &mut W,
1958 header: MavHeader,
1959 data: &M,
1960) -> Result<usize, error::MessageWriteError> {
1961 let mut message_raw = MAVLinkV1MessageRaw::new();
1962 message_raw.serialize_message(header, data);
1963
1964 let payload_length: usize = message_raw.payload_length().into();
1965 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1966
1967 w.write_all(&message_raw.0[..len]).await?;
1968
1969 Ok(len)
1970}
1971
1972#[cfg(feature = "embedded")]
1977pub async fn write_v1_msg_async<M: Message>(
1978 w: &mut impl embedded_io_async::Write,
1979 header: MavHeader,
1980 data: &M,
1981) -> Result<usize, error::MessageWriteError> {
1982 let mut message_raw = MAVLinkV1MessageRaw::new();
1983 message_raw.serialize_message(header, data);
1984
1985 let payload_length: usize = message_raw.payload_length().into();
1986 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1987
1988 w.write_all(&message_raw.0[..len])
1989 .await
1990 .map_err(|_| error::MessageWriteError::Io)?;
1991
1992 Ok(len)
1993}