1#![cfg_attr(not(feature = "std"), no_std)]
23#![cfg_attr(docsrs, feature(doc_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::{
43 bytes::Bytes,
44 error::{MessageWriteError, ParserError},
45};
46
47use crc_any::CRCu16;
48
49#[doc(hidden)]
50pub mod bytes;
51#[doc(hidden)]
52pub mod bytes_mut;
53#[cfg(feature = "std")]
54mod connection;
55pub mod error;
56#[cfg(feature = "std")]
57pub use self::connection::{connect, Connectable, MavConnection};
58
59#[cfg(feature = "tokio-1")]
60mod async_connection;
61#[cfg(feature = "tokio-1")]
62pub use self::async_connection::{connect_async, AsyncConnectable, AsyncMavConnection};
63
64#[cfg(feature = "tokio-1")]
65pub mod async_peek_reader;
66#[cfg(feature = "tokio-1")]
67use async_peek_reader::AsyncPeekReader;
68#[cfg(feature = "tokio-1")]
69use tokio::io::{AsyncWrite, AsyncWriteExt};
70
71#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
72pub mod embedded;
73#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))]
74use embedded::{Read, Write};
75
76#[cfg(not(feature = "signing"))]
77type SigningData = ();
78#[cfg(feature = "signing")]
79mod signing;
80#[cfg(feature = "signing")]
81pub use self::signing::{SigningConfig, SigningData};
82#[cfg(feature = "signing")]
83use sha2::{Digest, Sha256};
84
85#[cfg(feature = "arbitrary")]
86use arbitrary::Arbitrary;
87
88#[cfg(any(feature = "std", feature = "tokio-1"))]
89mod connectable;
90
91#[cfg(any(feature = "std", feature = "tokio-1"))]
92pub use connectable::ConnectionAddress;
93
94#[cfg(feature = "direct-serial")]
95pub use connection::direct_serial::config::SerialConfig;
96
97#[cfg(feature = "tcp")]
98pub use connection::tcp::config::{TcpConfig, TcpMode};
99
100#[cfg(feature = "udp")]
101pub use connection::udp::config::{UdpConfig, UdpMode};
102
103#[cfg(feature = "std")]
104pub use connection::file::config::FileConfig;
105
106pub const MAX_FRAME_SIZE: usize = 280;
110
111pub trait Message
116where
117 Self: Sized,
118{
119 fn message_id(&self) -> u32;
121
122 fn message_name(&self) -> &'static str;
124
125 fn target_system_id(&self) -> Option<u8>;
127
128 fn target_component_id(&self) -> Option<u8>;
130
131 fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize;
133
134 fn parse(
136 version: MavlinkVersion,
137 msgid: u32,
138 payload: &[u8],
139 ) -> Result<Self, error::ParserError>;
140
141 fn message_id_from_name(name: &str) -> Option<u32>;
143 fn default_message_from_id(id: u32) -> Option<Self>;
145 #[cfg(feature = "arbitrary")]
147 fn random_message_from_id<R: rand::RngCore>(id: u32, rng: &mut R) -> Option<Self>;
148 fn extra_crc(id: u32) -> u8;
150}
151
152pub trait MessageData: Sized {
153 type Message: Message;
154
155 const ID: u32;
156 const NAME: &'static str;
157 const EXTRA_CRC: u8;
158 const ENCODED_LEN: usize;
159
160 fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize;
161 fn deser(version: MavlinkVersion, payload: &[u8]) -> Result<Self, ParserError>;
162}
163
164#[derive(Debug, Copy, Clone, PartialEq, Eq)]
166#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
167#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
168pub struct MavHeader {
169 pub system_id: u8,
171 pub component_id: u8,
173 pub sequence: u8,
175}
176
177#[derive(Debug, Copy, Clone, PartialEq, Eq)]
179#[cfg_attr(feature = "serde", derive(Serialize))]
180#[cfg_attr(feature = "serde", serde(tag = "type"))]
181#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
182pub enum MavlinkVersion {
183 V1,
185 V2,
187}
188
189pub const MAV_STX: u8 = 0xFE;
191
192pub const MAV_STX_V2: u8 = 0xFD;
194
195impl Default for MavHeader {
198 fn default() -> Self {
199 Self {
200 system_id: 255,
201 component_id: 0,
202 sequence: 0,
203 }
204 }
205}
206
207#[derive(Debug, Clone)]
211#[cfg_attr(feature = "serde", derive(Serialize))]
212#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
213pub struct MavFrame<M: Message> {
214 pub header: MavHeader,
216 pub msg: M,
218 pub protocol_version: MavlinkVersion,
220}
221
222impl<M: Message> MavFrame<M> {
223 pub fn ser(&self, buf: &mut [u8]) -> usize {
227 let mut buf = bytes_mut::BytesMut::new(buf);
228
229 let mut payload_buf = [0u8; 255];
231 let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
232
233 buf.put_u8(self.header.sequence);
252 buf.put_u8(self.header.system_id);
253 buf.put_u8(self.header.component_id);
254
255 match self.protocol_version {
257 MavlinkVersion::V2 => {
258 let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
259 buf.put_slice(&bytes[..3]);
260 }
261 MavlinkVersion::V1 => {
262 buf.put_u8(self.msg.message_id() as u8); }
264 }
265
266 buf.put_slice(&payload_buf[..payload_len]);
267 buf.len()
268 }
269
270 pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result<Self, ParserError> {
274 let mut buf = Bytes::new(input);
275
276 let sequence = buf.get_u8();
286 let system_id = buf.get_u8();
287 let component_id = buf.get_u8();
288 let header = MavHeader {
289 system_id,
290 component_id,
291 sequence,
292 };
293
294 let msg_id = match version {
295 MavlinkVersion::V2 => buf.get_u24_le(),
296 MavlinkVersion::V1 => buf.get_u8().into(),
297 };
298
299 M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self {
300 header,
301 msg,
302 protocol_version: version,
303 })
304 }
305
306 pub fn header(&self) -> MavHeader {
308 self.header
309 }
310}
311
312pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 {
314 let mut crc_calculator = CRCu16::crc16mcrf4cc();
315 crc_calculator.digest(data);
316
317 crc_calculator.digest(&[extra_crc]);
318 crc_calculator.get_crc()
319}
320
321#[derive(Debug, Clone, Copy, PartialEq, Eq)]
322pub enum ReadVersion {
324 Single(MavlinkVersion),
326 Any,
328}
329
330impl ReadVersion {
331 #[cfg(feature = "std")]
332 fn from_conn_cfg<C: MavConnection<M>, M: Message>(conn: &C) -> Self {
333 if conn.allow_recv_any_version() {
334 Self::Any
335 } else {
336 conn.protocol_version().into()
337 }
338 }
339 #[cfg(feature = "tokio-1")]
340 fn from_async_conn_cfg<C: AsyncMavConnection<M>, M: Message + Sync + Send>(conn: &C) -> Self {
341 if conn.allow_recv_any_version() {
342 Self::Any
343 } else {
344 conn.protocol_version().into()
345 }
346 }
347}
348
349impl From<MavlinkVersion> for ReadVersion {
350 fn from(value: MavlinkVersion) -> Self {
351 Self::Single(value)
352 }
353}
354
355pub fn read_versioned_msg<M: Message, R: Read>(
357 r: &mut PeekReader<R>,
358 version: ReadVersion,
359) -> Result<(MavHeader, M), error::MessageReadError> {
360 match version {
361 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg(r),
362 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
363 ReadVersion::Any => read_any_msg(r),
364 }
365}
366
367pub fn read_raw_versioned_msg<M: Message, R: Read>(
369 r: &mut PeekReader<R>,
370 version: ReadVersion,
371) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
372 match version {
373 ReadVersion::Single(MavlinkVersion::V2) => {
374 Ok(MAVLinkMessageRaw::V2(read_v2_raw_message::<M, _>(r)?))
375 }
376 ReadVersion::Single(MavlinkVersion::V1) => {
377 Ok(MAVLinkMessageRaw::V1(read_v1_raw_message::<M, _>(r)?))
378 }
379 ReadVersion::Any => read_any_raw_message::<M, _>(r),
380 }
381}
382
383#[cfg(feature = "tokio-1")]
385pub async fn read_versioned_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
386 r: &mut AsyncPeekReader<R>,
387 version: ReadVersion,
388) -> Result<(MavHeader, M), error::MessageReadError> {
389 match version {
390 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async(r).await,
391 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
392 ReadVersion::Any => read_any_msg_async(r).await,
393 }
394}
395
396#[cfg(feature = "tokio-1")]
398pub async fn read_raw_versioned_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
399 r: &mut AsyncPeekReader<R>,
400 version: ReadVersion,
401) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
402 match version {
403 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
404 read_v2_raw_message_async::<M, _>(r).await?,
405 )),
406 ReadVersion::Single(MavlinkVersion::V1) => Ok(MAVLinkMessageRaw::V1(
407 read_v1_raw_message_async::<M, _>(r).await?,
408 )),
409 ReadVersion::Any => read_any_raw_message_async::<M, _>(r).await,
410 }
411}
412
413#[cfg(feature = "signing")]
418pub fn read_raw_versioned_msg_signed<M: Message, R: Read>(
419 r: &mut PeekReader<R>,
420 version: ReadVersion,
421 signing_data: Option<&SigningData>,
422) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
423 match version {
424 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
425 read_v2_raw_message_inner::<M, _>(r, signing_data)?,
426 )),
427 ReadVersion::Single(MavlinkVersion::V1) => {
428 Ok(MAVLinkMessageRaw::V1(read_v1_raw_message::<M, _>(r)?))
429 }
430 ReadVersion::Any => read_any_raw_message_inner::<M, _>(r, signing_data),
431 }
432}
433
434#[cfg(feature = "signing")]
439pub fn read_versioned_msg_signed<M: Message, R: Read>(
440 r: &mut PeekReader<R>,
441 version: ReadVersion,
442 signing_data: Option<&SigningData>,
443) -> Result<(MavHeader, M), error::MessageReadError> {
444 match version {
445 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_inner(r, signing_data),
446 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
447 ReadVersion::Any => read_any_msg_inner(r, signing_data),
448 }
449}
450
451#[cfg(all(feature = "tokio-1", feature = "signing"))]
456pub async fn read_raw_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
457 r: &mut AsyncPeekReader<R>,
458 version: ReadVersion,
459 signing_data: Option<&SigningData>,
460) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
461 match version {
462 ReadVersion::Single(MavlinkVersion::V2) => Ok(MAVLinkMessageRaw::V2(
463 read_v2_raw_message_async_inner::<M, _>(r, signing_data).await?,
464 )),
465 ReadVersion::Single(MavlinkVersion::V1) => Ok(MAVLinkMessageRaw::V1(
466 read_v1_raw_message_async::<M, _>(r).await?,
467 )),
468 ReadVersion::Any => read_any_raw_message_async_inner::<M, _>(r, signing_data).await,
469 }
470}
471
472#[cfg(all(feature = "tokio-1", feature = "signing"))]
477pub async fn read_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
478 r: &mut AsyncPeekReader<R>,
479 version: ReadVersion,
480 signing_data: Option<&SigningData>,
481) -> Result<(MavHeader, M), error::MessageReadError> {
482 match version {
483 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async_inner(r, signing_data).await,
484 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
485 ReadVersion::Any => read_any_msg_async_inner(r, signing_data).await,
486 }
487}
488
489#[derive(Debug, Copy, Clone, PartialEq, Eq)]
490pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]);
495
496impl Default for MAVLinkV1MessageRaw {
497 fn default() -> Self {
498 Self::new()
499 }
500}
501
502impl MAVLinkV1MessageRaw {
503 const HEADER_SIZE: usize = 5;
504
505 pub const fn new() -> Self {
507 Self([0; 1 + Self::HEADER_SIZE + 255 + 2])
508 }
509
510 pub const fn from_bytes_unparsed(bytes: [u8; 1 + Self::HEADER_SIZE + 255 + 2]) -> Self {
514 Self(bytes)
515 }
516
517 #[inline]
519 pub fn as_slice(&self) -> &[u8] {
520 &self.0
521 }
522
523 #[inline]
525 pub fn as_mut_slice(&mut self) -> &mut [u8] {
526 &mut self.0
527 }
528
529 #[inline]
531 pub fn into_inner(self) -> [u8; 1 + Self::HEADER_SIZE + 255 + 2] {
532 self.0
533 }
534
535 #[inline]
537 pub fn header(&self) -> &[u8] {
538 &self.0[1..=Self::HEADER_SIZE]
539 }
540
541 #[inline]
543 fn mut_header(&mut self) -> &mut [u8] {
544 &mut self.0[1..=Self::HEADER_SIZE]
545 }
546
547 #[inline]
549 pub fn payload_length(&self) -> u8 {
550 self.0[1]
551 }
552
553 #[inline]
555 pub fn sequence(&self) -> u8 {
556 self.0[2]
557 }
558
559 #[inline]
561 pub fn system_id(&self) -> u8 {
562 self.0[3]
563 }
564
565 #[inline]
567 pub fn component_id(&self) -> u8 {
568 self.0[4]
569 }
570
571 #[inline]
573 pub fn message_id(&self) -> u8 {
574 self.0[5]
575 }
576
577 #[inline]
579 pub fn payload(&self) -> &[u8] {
580 let payload_length: usize = self.payload_length().into();
581 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
582 }
583
584 #[inline]
586 pub fn checksum(&self) -> u16 {
587 let payload_length: usize = self.payload_length().into();
588 u16::from_le_bytes([
589 self.0[1 + Self::HEADER_SIZE + payload_length],
590 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
591 ])
592 }
593
594 #[inline]
595 fn mut_payload_and_checksum(&mut self) -> &mut [u8] {
596 let payload_length: usize = self.payload_length().into();
597 &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)]
598 }
599
600 #[inline]
602 pub fn has_valid_crc<M: Message>(&self) -> bool {
603 let payload_length: usize = self.payload_length().into();
604 self.checksum()
605 == calculate_crc(
606 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
607 M::extra_crc(self.message_id().into()),
608 )
609 }
610
611 pub fn raw_bytes(&self) -> &[u8] {
613 let payload_length = self.payload_length() as usize;
614 &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)]
615 }
616
617 fn serialize_stx_and_header_and_crc(
621 &mut self,
622 header: MavHeader,
623 msgid: u32,
624 payload_length: usize,
625 extra_crc: u8,
626 ) {
627 self.0[0] = MAV_STX;
628
629 let header_buf = self.mut_header();
630 header_buf.copy_from_slice(&[
631 payload_length as u8,
632 header.sequence,
633 header.system_id,
634 header.component_id,
635 msgid.try_into().unwrap(),
636 ]);
637
638 let crc = calculate_crc(
639 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
640 extra_crc,
641 );
642 self.0[(1 + Self::HEADER_SIZE + payload_length)
643 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
644 .copy_from_slice(&crc.to_le_bytes());
645 }
646
647 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
653 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
654 let payload_length = message.ser(MavlinkVersion::V1, payload_buf);
655
656 let message_id = message.message_id();
657 self.serialize_stx_and_header_and_crc(
658 header,
659 message_id,
660 payload_length,
661 M::extra_crc(message_id),
662 );
663 }
664
665 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
669 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
670 let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf);
671
672 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC);
673 }
674}
675
676fn try_decode_v1<M: Message, R: Read>(
677 reader: &mut PeekReader<R>,
678) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
679 let mut message = MAVLinkV1MessageRaw::new();
680 let whole_header_size = MAVLinkV1MessageRaw::HEADER_SIZE + 1;
681
682 message.0[0] = MAV_STX;
683 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
684 message.mut_header().copy_from_slice(header);
685 let packet_length = message.raw_bytes().len();
686 let payload_and_checksum = &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
687 message
688 .mut_payload_and_checksum()
689 .copy_from_slice(payload_and_checksum);
690
691 if message.has_valid_crc::<M>() {
694 reader.consume(message.raw_bytes().len());
695 Ok(Some(message))
696 } else {
697 Ok(None)
698 }
699}
700
701#[cfg(feature = "tokio-1")]
702async fn try_decode_v1_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
704 reader: &mut AsyncPeekReader<R>,
705) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
706 let mut message = MAVLinkV1MessageRaw::new();
707
708 message.0[0] = MAV_STX;
709 let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await?
710 [..MAVLinkV1MessageRaw::HEADER_SIZE];
711 message.mut_header().copy_from_slice(header);
712 let packet_length = message.raw_bytes().len() - 1;
713 let payload_and_checksum =
714 &reader.peek_exact(packet_length).await?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length];
715 message
716 .mut_payload_and_checksum()
717 .copy_from_slice(payload_and_checksum);
718
719 if message.has_valid_crc::<M>() {
722 reader.consume(message.raw_bytes().len() - 1);
723 Ok(Some(message))
724 } else {
725 Ok(None)
726 }
727}
728
729pub fn read_v1_raw_message<M: Message, R: Read>(
731 reader: &mut PeekReader<R>,
732) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
733 loop {
734 while reader.peek_exact(1)?[0] != MAV_STX {
736 reader.consume(1);
737 }
738
739 if let Some(msg) = try_decode_v1::<M, _>(reader)? {
740 return Ok(msg);
741 }
742
743 reader.consume(1);
744 }
745}
746
747#[cfg(feature = "tokio-1")]
749pub async fn read_v1_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
750 reader: &mut AsyncPeekReader<R>,
751) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
752 loop {
753 loop {
754 if reader.read_u8().await? == MAV_STX {
756 break;
757 }
758 }
759
760 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
761 return Ok(message);
762 }
763 }
764}
765
766#[cfg(feature = "embedded")]
772pub async fn read_v1_raw_message_async<M: Message>(
773 reader: &mut impl embedded_io_async::Read,
774) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
775 loop {
776 let mut byte = [0u8];
778 loop {
779 reader
780 .read_exact(&mut byte)
781 .await
782 .map_err(|_| error::MessageReadError::Io)?;
783 if byte[0] == MAV_STX {
784 break;
785 }
786 }
787
788 let mut message = MAVLinkV1MessageRaw::new();
789
790 message.0[0] = MAV_STX;
791 reader
792 .read_exact(message.mut_header())
793 .await
794 .map_err(|_| error::MessageReadError::Io)?;
795 reader
796 .read_exact(message.mut_payload_and_checksum())
797 .await
798 .map_err(|_| error::MessageReadError::Io)?;
799
800 if message.has_valid_crc::<M>() {
803 return Ok(message);
804 }
805 }
806}
807
808pub fn read_v1_msg<M: Message, R: Read>(
810 r: &mut PeekReader<R>,
811) -> Result<(MavHeader, M), error::MessageReadError> {
812 let message = read_v1_raw_message::<M, _>(r)?;
813
814 Ok((
815 MavHeader {
816 sequence: message.sequence(),
817 system_id: message.system_id(),
818 component_id: message.component_id(),
819 },
820 M::parse(
821 MavlinkVersion::V1,
822 u32::from(message.message_id()),
823 message.payload(),
824 )?,
825 ))
826}
827
828#[cfg(feature = "tokio-1")]
830pub async fn read_v1_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
831 r: &mut AsyncPeekReader<R>,
832) -> Result<(MavHeader, M), error::MessageReadError> {
833 let message = read_v1_raw_message_async::<M, _>(r).await?;
834
835 Ok((
836 MavHeader {
837 sequence: message.sequence(),
838 system_id: message.system_id(),
839 component_id: message.component_id(),
840 },
841 M::parse(
842 MavlinkVersion::V1,
843 u32::from(message.message_id()),
844 message.payload(),
845 )?,
846 ))
847}
848
849#[cfg(feature = "embedded")]
854pub async fn read_v1_msg_async<M: Message>(
855 r: &mut impl embedded_io_async::Read,
856) -> Result<(MavHeader, M), error::MessageReadError> {
857 let message = read_v1_raw_message_async::<M>(r).await?;
858
859 Ok((
860 MavHeader {
861 sequence: message.sequence(),
862 system_id: message.system_id(),
863 component_id: message.component_id(),
864 },
865 M::parse(
866 MavlinkVersion::V1,
867 u32::from(message.message_id()),
868 message.payload(),
869 )?,
870 ))
871}
872
873const MAVLINK_IFLAG_SIGNED: u8 = 0x01;
874const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED;
875
876#[derive(Debug, Copy, Clone, PartialEq, Eq)]
877pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]);
882
883impl Default for MAVLinkV2MessageRaw {
884 fn default() -> Self {
885 Self::new()
886 }
887}
888
889impl MAVLinkV2MessageRaw {
890 const HEADER_SIZE: usize = 9;
891 const SIGNATURE_SIZE: usize = 13;
892
893 pub const fn new() -> Self {
895 Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE])
896 }
897
898 pub const fn from_bytes_unparsed(
902 bytes: [u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE],
903 ) -> Self {
904 Self(bytes)
905 }
906
907 #[inline]
909 pub fn as_slice(&self) -> &[u8] {
910 &self.0
911 }
912
913 #[inline]
915 pub fn as_mut_slice(&mut self) -> &mut [u8] {
916 &mut self.0
917 }
918
919 #[inline]
921 pub fn into_inner(self) -> [u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE] {
922 self.0
923 }
924
925 #[inline]
927 pub fn header(&self) -> &[u8] {
928 &self.0[1..=Self::HEADER_SIZE]
929 }
930
931 #[inline]
933 fn mut_header(&mut self) -> &mut [u8] {
934 &mut self.0[1..=Self::HEADER_SIZE]
935 }
936
937 #[inline]
939 pub fn payload_length(&self) -> u8 {
940 self.0[1]
941 }
942
943 #[inline]
947 pub fn incompatibility_flags(&self) -> u8 {
948 self.0[2]
949 }
950
951 #[inline]
955 pub fn incompatibility_flags_mut(&mut self) -> &mut u8 {
956 &mut self.0[2]
957 }
958
959 #[inline]
961 pub fn compatibility_flags(&self) -> u8 {
962 self.0[3]
963 }
964
965 #[inline]
967 pub fn sequence(&self) -> u8 {
968 self.0[4]
969 }
970
971 #[inline]
973 pub fn system_id(&self) -> u8 {
974 self.0[5]
975 }
976
977 #[inline]
979 pub fn component_id(&self) -> u8 {
980 self.0[6]
981 }
982
983 #[inline]
985 pub fn message_id(&self) -> u32 {
986 u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0])
987 }
988
989 #[inline]
991 pub fn payload(&self) -> &[u8] {
992 let payload_length: usize = self.payload_length().into();
993 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
994 }
995
996 #[inline]
998 pub fn checksum(&self) -> u16 {
999 let payload_length: usize = self.payload_length().into();
1000 u16::from_le_bytes([
1001 self.0[1 + Self::HEADER_SIZE + payload_length],
1002 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
1003 ])
1004 }
1005
1006 #[cfg(feature = "signing")]
1008 #[inline]
1009 pub fn checksum_bytes(&self) -> &[u8] {
1010 let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize;
1011 &self.0[checksum_offset..(checksum_offset + 2)]
1012 }
1013
1014 #[cfg(feature = "signing")]
1018 #[inline]
1019 pub fn signature_link_id(&self) -> u8 {
1020 let payload_length: usize = self.payload_length().into();
1021 self.0[1 + Self::HEADER_SIZE + payload_length + 2]
1022 }
1023
1024 #[cfg(feature = "signing")]
1026 #[inline]
1027 pub fn signature_link_id_mut(&mut self) -> &mut u8 {
1028 let payload_length: usize = self.payload_length().into();
1029 &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2]
1030 }
1031
1032 #[cfg(feature = "signing")]
1038 #[inline]
1039 pub fn signature_timestamp(&self) -> u64 {
1040 let mut timestamp_bytes = [0u8; 8];
1041 timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes());
1042 u64::from_le_bytes(timestamp_bytes)
1043 }
1044
1045 #[cfg(feature = "signing")]
1049 #[inline]
1050 pub fn signature_timestamp_bytes(&self) -> &[u8] {
1051 let payload_length: usize = self.payload_length().into();
1052 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
1053 &self.0[timestamp_start..(timestamp_start + 6)]
1054 }
1055
1056 #[cfg(feature = "signing")]
1058 #[inline]
1059 pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] {
1060 let payload_length: usize = self.payload_length().into();
1061 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
1062 &mut self.0[timestamp_start..(timestamp_start + 6)]
1063 }
1064
1065 #[cfg(feature = "signing")]
1069 #[inline]
1070 pub fn signature_value(&self) -> &[u8] {
1071 let payload_length: usize = self.payload_length().into();
1072 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
1073 &self.0[signature_start..(signature_start + 6)]
1074 }
1075
1076 #[cfg(feature = "signing")]
1078 #[inline]
1079 pub fn signature_value_mut(&mut self) -> &mut [u8] {
1080 let payload_length: usize = self.payload_length().into();
1081 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
1082 &mut self.0[signature_start..(signature_start + 6)]
1083 }
1084
1085 fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] {
1086 let payload_length: usize = self.payload_length().into();
1087
1088 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
1090 0
1091 } else {
1092 Self::SIGNATURE_SIZE
1093 };
1094
1095 &mut self.0
1096 [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
1097 }
1098
1099 #[inline]
1101 pub fn has_valid_crc<M: Message>(&self) -> bool {
1102 let payload_length: usize = self.payload_length().into();
1103 self.checksum()
1104 == calculate_crc(
1105 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
1106 M::extra_crc(self.message_id()),
1107 )
1108 }
1109
1110 #[cfg(feature = "signing")]
1114 pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) {
1115 let mut hasher = Sha256::new();
1116 hasher.update(secret_key);
1117 hasher.update([MAV_STX_V2]);
1118 hasher.update(self.header());
1119 hasher.update(self.payload());
1120 hasher.update(self.checksum_bytes());
1121 hasher.update([self.signature_link_id()]);
1122 hasher.update(self.signature_timestamp_bytes());
1123 target_buffer.copy_from_slice(&hasher.finalize()[0..6]);
1124 }
1125
1126 pub fn raw_bytes(&self) -> &[u8] {
1128 let payload_length = self.payload_length() as usize;
1129
1130 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
1131 0
1132 } else {
1133 Self::SIGNATURE_SIZE
1134 };
1135
1136 &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
1137 }
1138
1139 fn serialize_stx_and_header_and_crc(
1140 &mut self,
1141 header: MavHeader,
1142 msgid: u32,
1143 payload_length: usize,
1144 extra_crc: u8,
1145 incompat_flags: u8,
1146 ) {
1147 self.0[0] = MAV_STX_V2;
1148 let msgid_bytes = msgid.to_le_bytes();
1149
1150 let header_buf = self.mut_header();
1151 header_buf.copy_from_slice(&[
1152 payload_length as u8,
1153 incompat_flags,
1154 0, header.sequence,
1156 header.system_id,
1157 header.component_id,
1158 msgid_bytes[0],
1159 msgid_bytes[1],
1160 msgid_bytes[2],
1161 ]);
1162
1163 let crc = calculate_crc(
1164 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
1165 extra_crc,
1166 );
1167 self.0[(1 + Self::HEADER_SIZE + payload_length)
1168 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
1169 .copy_from_slice(&crc.to_le_bytes());
1170 }
1171
1172 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
1176 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1177 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1178
1179 let message_id = message.message_id();
1180 self.serialize_stx_and_header_and_crc(
1181 header,
1182 message_id,
1183 payload_length,
1184 M::extra_crc(message_id),
1185 0,
1186 );
1187 }
1188
1189 pub fn serialize_message_for_signing<M: Message>(&mut self, header: MavHeader, message: &M) {
1194 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1195 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1196
1197 let message_id = message.message_id();
1198 self.serialize_stx_and_header_and_crc(
1199 header,
1200 message_id,
1201 payload_length,
1202 M::extra_crc(message_id),
1203 MAVLINK_IFLAG_SIGNED,
1204 );
1205 }
1206
1207 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
1208 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1209 let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf);
1210
1211 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0);
1212 }
1213}
1214
1215#[allow(unused_variables)]
1216fn try_decode_v2<M: Message, R: Read>(
1217 reader: &mut PeekReader<R>,
1218 signing_data: Option<&SigningData>,
1219) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1220 let mut message = MAVLinkV2MessageRaw::new();
1221 let whole_header_size = MAVLinkV2MessageRaw::HEADER_SIZE + 1;
1222
1223 message.0[0] = MAV_STX_V2;
1224 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
1225 message.mut_header().copy_from_slice(header);
1226
1227 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1228 reader.consume(1);
1230 return Ok(None);
1231 }
1232
1233 let packet_length = message.raw_bytes().len();
1234 let payload_and_checksum_and_sign =
1235 &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
1236 message
1237 .mut_payload_and_checksum_and_sign()
1238 .copy_from_slice(payload_and_checksum_and_sign);
1239
1240 if message.has_valid_crc::<M>() {
1241 reader.consume(message.raw_bytes().len());
1243 } else {
1244 reader.consume(1);
1245 return Ok(None);
1246 }
1247
1248 #[cfg(feature = "signing")]
1249 if let Some(signing_data) = signing_data {
1250 if !signing_data.verify_signature(&message) {
1251 return Ok(None);
1252 }
1253 }
1254
1255 Ok(Some(message))
1256}
1257
1258#[cfg(feature = "tokio-1")]
1259#[allow(unused_variables)]
1260async fn try_decode_v2_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1262 reader: &mut AsyncPeekReader<R>,
1263 signing_data: Option<&SigningData>,
1264) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1265 let mut message = MAVLinkV2MessageRaw::new();
1266
1267 message.0[0] = MAV_STX_V2;
1268 let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE).await?
1269 [..MAVLinkV2MessageRaw::HEADER_SIZE];
1270 message.mut_header().copy_from_slice(header);
1271
1272 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1273 return Ok(None);
1275 }
1276
1277 let packet_length = message.raw_bytes().len() - 1;
1278 let payload_and_checksum_and_sign =
1279 &reader.peek_exact(packet_length).await?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length];
1280 message
1281 .mut_payload_and_checksum_and_sign()
1282 .copy_from_slice(payload_and_checksum_and_sign);
1283
1284 if message.has_valid_crc::<M>() {
1285 reader.consume(message.raw_bytes().len() - 1);
1287 } else {
1288 return Ok(None);
1289 }
1290
1291 #[cfg(feature = "signing")]
1292 if let Some(signing_data) = signing_data {
1293 if !signing_data.verify_signature(&message) {
1294 return Ok(None);
1295 }
1296 }
1297
1298 Ok(Some(message))
1299}
1300
1301#[inline]
1303pub fn read_v2_raw_message<M: Message, R: Read>(
1304 reader: &mut PeekReader<R>,
1305) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1306 read_v2_raw_message_inner::<M, R>(reader, None)
1307}
1308
1309#[cfg(feature = "signing")]
1311#[inline]
1312pub fn read_v2_raw_message_signed<M: Message, R: Read>(
1313 reader: &mut PeekReader<R>,
1314 signing_data: Option<&SigningData>,
1315) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1316 read_v2_raw_message_inner::<M, R>(reader, signing_data)
1317}
1318
1319#[allow(unused_variables)]
1320fn read_v2_raw_message_inner<M: Message, R: Read>(
1321 reader: &mut PeekReader<R>,
1322 signing_data: Option<&SigningData>,
1323) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1324 loop {
1325 while reader.peek_exact(1)?[0] != MAV_STX_V2 {
1327 reader.consume(1);
1328 }
1329
1330 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1331 return Ok(message);
1332 }
1333 }
1334}
1335
1336#[cfg(feature = "tokio-1")]
1338pub async fn read_v2_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1339 reader: &mut AsyncPeekReader<R>,
1340) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1341 read_v2_raw_message_async_inner::<M, R>(reader, None).await
1342}
1343
1344#[cfg(feature = "tokio-1")]
1345#[allow(unused_variables)]
1346async fn read_v2_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1347 reader: &mut AsyncPeekReader<R>,
1348 signing_data: Option<&SigningData>,
1349) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1350 loop {
1351 loop {
1352 if reader.read_u8().await? == MAV_STX_V2 {
1354 break;
1355 }
1356 }
1357
1358 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1359 return Ok(message);
1360 }
1361 }
1362}
1363
1364#[cfg(all(feature = "tokio-1", feature = "signing"))]
1366pub async fn read_v2_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1367 reader: &mut AsyncPeekReader<R>,
1368 signing_data: Option<&SigningData>,
1369) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1370 read_v2_raw_message_async_inner::<M, R>(reader, signing_data).await
1371}
1372
1373#[cfg(feature = "embedded")]
1378pub async fn read_v2_raw_message_async<M: Message>(
1379 reader: &mut impl embedded_io_async::Read,
1380) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1381 loop {
1382 let mut byte = [0u8];
1384 loop {
1385 reader
1386 .read_exact(&mut byte)
1387 .await
1388 .map_err(|_| error::MessageReadError::Io)?;
1389 if byte[0] == MAV_STX_V2 {
1390 break;
1391 }
1392 }
1393
1394 let mut message = MAVLinkV2MessageRaw::new();
1395
1396 message.0[0] = MAV_STX_V2;
1397 reader
1398 .read_exact(message.mut_header())
1399 .await
1400 .map_err(|_| error::MessageReadError::Io)?;
1401
1402 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1403 continue;
1405 }
1406
1407 reader
1408 .read_exact(message.mut_payload_and_checksum_and_sign())
1409 .await
1410 .map_err(|_| error::MessageReadError::Io)?;
1411
1412 if message.has_valid_crc::<M>() {
1415 return Ok(message);
1416 }
1417 }
1418}
1419
1420#[inline]
1422pub fn read_v2_msg<M: Message, R: Read>(
1423 read: &mut PeekReader<R>,
1424) -> Result<(MavHeader, M), error::MessageReadError> {
1425 read_v2_msg_inner(read, None)
1426}
1427
1428#[cfg(feature = "signing")]
1430#[inline]
1431pub fn read_v2_msg_signed<M: Message, R: Read>(
1432 read: &mut PeekReader<R>,
1433 signing_data: Option<&SigningData>,
1434) -> Result<(MavHeader, M), error::MessageReadError> {
1435 read_v2_msg_inner(read, signing_data)
1436}
1437
1438fn read_v2_msg_inner<M: Message, R: Read>(
1439 read: &mut PeekReader<R>,
1440 signing_data: Option<&SigningData>,
1441) -> Result<(MavHeader, M), error::MessageReadError> {
1442 let message = read_v2_raw_message_inner::<M, _>(read, signing_data)?;
1443
1444 Ok((
1445 MavHeader {
1446 sequence: message.sequence(),
1447 system_id: message.system_id(),
1448 component_id: message.component_id(),
1449 },
1450 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1451 ))
1452}
1453
1454#[cfg(feature = "tokio-1")]
1456pub async fn read_v2_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1457 read: &mut AsyncPeekReader<R>,
1458) -> Result<(MavHeader, M), error::MessageReadError> {
1459 read_v2_msg_async_inner(read, None).await
1460}
1461
1462#[cfg(all(feature = "tokio-1", feature = "signing"))]
1464pub async fn read_v2_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1465 read: &mut AsyncPeekReader<R>,
1466 signing_data: Option<&SigningData>,
1467) -> Result<(MavHeader, M), error::MessageReadError> {
1468 read_v2_msg_async_inner(read, signing_data).await
1469}
1470
1471#[cfg(feature = "tokio-1")]
1472async fn read_v2_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1473 read: &mut AsyncPeekReader<R>,
1474 signing_data: Option<&SigningData>,
1475) -> Result<(MavHeader, M), error::MessageReadError> {
1476 let message = read_v2_raw_message_async_inner::<M, _>(read, signing_data).await?;
1477
1478 Ok((
1479 MavHeader {
1480 sequence: message.sequence(),
1481 system_id: message.system_id(),
1482 component_id: message.component_id(),
1483 },
1484 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1485 ))
1486}
1487
1488#[cfg(feature = "embedded")]
1493pub async fn read_v2_msg_async<M: Message, R: embedded_io_async::Read>(
1494 r: &mut R,
1495) -> Result<(MavHeader, M), error::MessageReadError> {
1496 let message = read_v2_raw_message_async::<M>(r).await?;
1497
1498 Ok((
1499 MavHeader {
1500 sequence: message.sequence(),
1501 system_id: message.system_id(),
1502 component_id: message.component_id(),
1503 },
1504 M::parse(
1505 MavlinkVersion::V2,
1506 u32::from(message.message_id()),
1507 message.payload(),
1508 )?,
1509 ))
1510}
1511
1512pub enum MAVLinkMessageRaw {
1514 V1(MAVLinkV1MessageRaw),
1515 V2(MAVLinkV2MessageRaw),
1516}
1517
1518impl MAVLinkMessageRaw {
1519 pub fn payload(&self) -> &[u8] {
1520 match self {
1521 Self::V1(msg) => msg.payload(),
1522 Self::V2(msg) => msg.payload(),
1523 }
1524 }
1525 pub fn sequence(&self) -> u8 {
1526 match self {
1527 Self::V1(msg) => msg.sequence(),
1528 Self::V2(msg) => msg.sequence(),
1529 }
1530 }
1531 pub fn system_id(&self) -> u8 {
1532 match self {
1533 Self::V1(msg) => msg.system_id(),
1534 Self::V2(msg) => msg.system_id(),
1535 }
1536 }
1537 pub fn component_id(&self) -> u8 {
1538 match self {
1539 Self::V1(msg) => msg.component_id(),
1540 Self::V2(msg) => msg.component_id(),
1541 }
1542 }
1543 pub fn message_id(&self) -> u32 {
1544 match self {
1545 Self::V1(msg) => u32::from(msg.message_id()),
1546 Self::V2(msg) => msg.message_id(),
1547 }
1548 }
1549 pub fn version(&self) -> MavlinkVersion {
1550 match self {
1551 Self::V1(_) => MavlinkVersion::V1,
1552 Self::V2(_) => MavlinkVersion::V2,
1553 }
1554 }
1555}
1556
1557#[inline]
1559pub fn read_any_raw_message<M: Message, R: Read>(
1560 reader: &mut PeekReader<R>,
1561) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1562 read_any_raw_message_inner::<M, R>(reader, None)
1563}
1564
1565#[cfg(feature = "signing")]
1567#[inline]
1568pub fn read_any_raw_message_signed<M: Message, R: Read>(
1569 reader: &mut PeekReader<R>,
1570 signing_data: Option<&SigningData>,
1571) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1572 read_any_raw_message_inner::<M, R>(reader, signing_data)
1573}
1574
1575#[allow(unused_variables)]
1576fn read_any_raw_message_inner<M: Message, R: Read>(
1577 reader: &mut PeekReader<R>,
1578 signing_data: Option<&SigningData>,
1579) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1580 loop {
1581 let version = loop {
1583 let byte = reader.peek_exact(1)?[0];
1584 if byte == MAV_STX {
1585 break MavlinkVersion::V1;
1586 }
1587 if byte == MAV_STX_V2 {
1588 break MavlinkVersion::V2;
1589 }
1590 reader.consume(1);
1591 };
1592 match version {
1593 MavlinkVersion::V1 => {
1594 if let Some(message) = try_decode_v1::<M, _>(reader)? {
1595 #[cfg(feature = "signing")]
1597 if let Some(signing) = signing_data {
1598 if signing.config.allow_unsigned {
1599 return Ok(MAVLinkMessageRaw::V1(message));
1600 }
1601 } else {
1602 return Ok(MAVLinkMessageRaw::V1(message));
1603 }
1604 #[cfg(not(feature = "signing"))]
1605 return Ok(MAVLinkMessageRaw::V1(message));
1606 }
1607
1608 reader.consume(1);
1609 }
1610 MavlinkVersion::V2 => {
1611 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1612 return Ok(MAVLinkMessageRaw::V2(message));
1613 }
1614 }
1615 }
1616 }
1617}
1618
1619#[cfg(feature = "tokio-1")]
1621pub async fn read_any_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1622 reader: &mut AsyncPeekReader<R>,
1623) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1624 read_any_raw_message_async_inner::<M, R>(reader, None).await
1625}
1626
1627#[cfg(all(feature = "tokio-1", feature = "signing"))]
1629pub async fn read_any_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1630 reader: &mut AsyncPeekReader<R>,
1631 signing_data: Option<&SigningData>,
1632) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1633 read_any_raw_message_async_inner::<M, R>(reader, signing_data).await
1634}
1635
1636#[cfg(feature = "tokio-1")]
1637#[allow(unused_variables)]
1638async fn read_any_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1639 reader: &mut AsyncPeekReader<R>,
1640 signing_data: Option<&SigningData>,
1641) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1642 loop {
1643 let version = loop {
1645 let read = reader.read_u8().await?;
1646 if read == MAV_STX {
1647 break MavlinkVersion::V1;
1648 }
1649 if read == MAV_STX_V2 {
1650 break MavlinkVersion::V2;
1651 }
1652 };
1653
1654 match version {
1655 MavlinkVersion::V1 => {
1656 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
1657 #[cfg(feature = "signing")]
1659 if let Some(signing) = signing_data {
1660 if signing.config.allow_unsigned {
1661 return Ok(MAVLinkMessageRaw::V1(message));
1662 }
1663 } else {
1664 return Ok(MAVLinkMessageRaw::V1(message));
1665 }
1666 #[cfg(not(feature = "signing"))]
1667 return Ok(MAVLinkMessageRaw::V1(message));
1668 }
1669 }
1670 MavlinkVersion::V2 => {
1671 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1672 return Ok(MAVLinkMessageRaw::V2(message));
1673 }
1674 }
1675 }
1676 }
1677}
1678
1679#[inline]
1681pub fn read_any_msg<M: Message, R: Read>(
1682 read: &mut PeekReader<R>,
1683) -> Result<(MavHeader, M), error::MessageReadError> {
1684 read_any_msg_inner(read, None)
1685}
1686
1687#[cfg(feature = "signing")]
1691#[inline]
1692pub fn read_any_msg_signed<M: Message, R: Read>(
1693 read: &mut PeekReader<R>,
1694 signing_data: Option<&SigningData>,
1695) -> Result<(MavHeader, M), error::MessageReadError> {
1696 read_any_msg_inner(read, signing_data)
1697}
1698
1699fn read_any_msg_inner<M: Message, R: Read>(
1700 read: &mut PeekReader<R>,
1701 signing_data: Option<&SigningData>,
1702) -> Result<(MavHeader, M), error::MessageReadError> {
1703 let message = read_any_raw_message_inner::<M, _>(read, signing_data)?;
1704 Ok((
1705 MavHeader {
1706 sequence: message.sequence(),
1707 system_id: message.system_id(),
1708 component_id: message.component_id(),
1709 },
1710 M::parse(message.version(), message.message_id(), message.payload())?,
1711 ))
1712}
1713
1714#[cfg(feature = "tokio-1")]
1716pub async fn read_any_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1717 read: &mut AsyncPeekReader<R>,
1718) -> Result<(MavHeader, M), error::MessageReadError> {
1719 read_any_msg_async_inner(read, None).await
1720}
1721
1722#[cfg(all(feature = "tokio-1", feature = "signing"))]
1726#[inline]
1727pub async fn read_any_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1728 read: &mut AsyncPeekReader<R>,
1729 signing_data: Option<&SigningData>,
1730) -> Result<(MavHeader, M), error::MessageReadError> {
1731 read_any_msg_async_inner(read, signing_data).await
1732}
1733
1734#[cfg(feature = "tokio-1")]
1735async fn read_any_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1736 read: &mut AsyncPeekReader<R>,
1737 signing_data: Option<&SigningData>,
1738) -> Result<(MavHeader, M), error::MessageReadError> {
1739 let message = read_any_raw_message_async_inner::<M, _>(read, signing_data).await?;
1740
1741 Ok((
1742 MavHeader {
1743 sequence: message.sequence(),
1744 system_id: message.system_id(),
1745 component_id: message.component_id(),
1746 },
1747 M::parse(message.version(), message.message_id(), message.payload())?,
1748 ))
1749}
1750
1751pub fn write_versioned_msg<M: Message, W: Write>(
1753 w: &mut W,
1754 version: MavlinkVersion,
1755 header: MavHeader,
1756 data: &M,
1757) -> Result<usize, error::MessageWriteError> {
1758 match version {
1759 MavlinkVersion::V2 => write_v2_msg(w, header, data),
1760 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1761 }
1762}
1763
1764#[cfg(feature = "signing")]
1768pub fn write_versioned_msg_signed<M: Message, W: Write>(
1769 w: &mut W,
1770 version: MavlinkVersion,
1771 header: MavHeader,
1772 data: &M,
1773 signing_data: Option<&SigningData>,
1774) -> Result<usize, error::MessageWriteError> {
1775 match version {
1776 MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data),
1777 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1778 }
1779}
1780
1781#[cfg(feature = "tokio-1")]
1783pub async fn write_versioned_msg_async<M: Message, W: AsyncWrite + Unpin>(
1784 w: &mut W,
1785 version: MavlinkVersion,
1786 header: MavHeader,
1787 data: &M,
1788) -> Result<usize, error::MessageWriteError> {
1789 match version {
1790 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1791 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1792 }
1793}
1794
1795#[cfg(all(feature = "tokio-1", feature = "signing"))]
1799pub async fn write_versioned_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1800 w: &mut W,
1801 version: MavlinkVersion,
1802 header: MavHeader,
1803 data: &M,
1804 signing_data: Option<&SigningData>,
1805) -> Result<usize, error::MessageWriteError> {
1806 match version {
1807 MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await,
1808 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1809 }
1810}
1811
1812#[cfg(feature = "embedded")]
1817pub async fn write_versioned_msg_async<M: Message>(
1818 w: &mut impl embedded_io_async::Write,
1819 version: MavlinkVersion,
1820 header: MavHeader,
1821 data: &M,
1822) -> Result<usize, error::MessageWriteError> {
1823 match version {
1824 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1825 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1826 }
1827}
1828
1829pub fn write_v2_msg<M: Message, W: Write>(
1831 w: &mut W,
1832 header: MavHeader,
1833 data: &M,
1834) -> Result<usize, error::MessageWriteError> {
1835 let mut message_raw = MAVLinkV2MessageRaw::new();
1836 message_raw.serialize_message(header, data);
1837
1838 let payload_length: usize = message_raw.payload_length().into();
1839 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1840
1841 w.write_all(&message_raw.0[..len])?;
1842
1843 Ok(len)
1844}
1845
1846#[cfg(feature = "signing")]
1848pub fn write_v2_msg_signed<M: Message, W: Write>(
1849 w: &mut W,
1850 header: MavHeader,
1851 data: &M,
1852 signing_data: Option<&SigningData>,
1853) -> Result<usize, error::MessageWriteError> {
1854 let mut message_raw = MAVLinkV2MessageRaw::new();
1855
1856 let signature_len = if let Some(signing_data) = signing_data {
1857 if signing_data.config.sign_outgoing {
1858 message_raw.serialize_message_for_signing(header, data);
1859 signing_data.sign_message(&mut message_raw);
1860 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1861 } else {
1862 message_raw.serialize_message(header, data);
1863 0
1864 }
1865 } else {
1866 message_raw.serialize_message(header, data);
1867 0
1868 };
1869
1870 let payload_length: usize = message_raw.payload_length().into();
1871 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1872
1873 w.write_all(&message_raw.0[..len])?;
1874
1875 Ok(len)
1876}
1877
1878#[cfg(feature = "tokio-1")]
1880pub async fn write_v2_msg_async<M: Message, W: AsyncWrite + Unpin>(
1881 w: &mut W,
1882 header: MavHeader,
1883 data: &M,
1884) -> Result<usize, error::MessageWriteError> {
1885 let mut message_raw = MAVLinkV2MessageRaw::new();
1886 message_raw.serialize_message(header, data);
1887
1888 let payload_length: usize = message_raw.payload_length().into();
1889 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1890
1891 w.write_all(&message_raw.0[..len]).await?;
1892
1893 Ok(len)
1894}
1895
1896#[cfg(feature = "signing")]
1898#[cfg(feature = "tokio-1")]
1899pub async fn write_v2_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1900 w: &mut W,
1901 header: MavHeader,
1902 data: &M,
1903 signing_data: Option<&SigningData>,
1904) -> Result<usize, error::MessageWriteError> {
1905 let mut message_raw = MAVLinkV2MessageRaw::new();
1906
1907 let signature_len = if let Some(signing_data) = signing_data {
1908 if signing_data.config.sign_outgoing {
1909 message_raw.serialize_message_for_signing(header, data);
1910 signing_data.sign_message(&mut message_raw);
1911 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1912 } else {
1913 message_raw.serialize_message(header, data);
1914 0
1915 }
1916 } else {
1917 message_raw.serialize_message(header, data);
1918 0
1919 };
1920
1921 let payload_length: usize = message_raw.payload_length().into();
1922 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1923
1924 w.write_all(&message_raw.0[..len]).await?;
1925
1926 Ok(len)
1927}
1928
1929#[cfg(feature = "embedded")]
1934pub async fn write_v2_msg_async<M: Message>(
1935 w: &mut impl embedded_io_async::Write,
1936 header: MavHeader,
1937 data: &M,
1938) -> Result<usize, error::MessageWriteError> {
1939 let mut message_raw = MAVLinkV2MessageRaw::new();
1940 message_raw.serialize_message(header, data);
1941
1942 let payload_length: usize = message_raw.payload_length().into();
1943 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1944
1945 w.write_all(&message_raw.0[..len])
1946 .await
1947 .map_err(|_| error::MessageWriteError::Io)?;
1948
1949 Ok(len)
1950}
1951
1952pub fn write_v1_msg<M: Message, W: Write>(
1954 w: &mut W,
1955 header: MavHeader,
1956 data: &M,
1957) -> Result<usize, error::MessageWriteError> {
1958 if data.message_id() > u8::MAX.into() {
1959 return Err(MessageWriteError::MAVLink2Only);
1960 }
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])?;
1968
1969 Ok(len)
1970}
1971
1972#[cfg(feature = "tokio-1")]
1974pub async fn write_v1_msg_async<M: Message, W: AsyncWrite + Unpin>(
1975 w: &mut W,
1976 header: MavHeader,
1977 data: &M,
1978) -> Result<usize, error::MessageWriteError> {
1979 if data.message_id() > u8::MAX.into() {
1980 return Err(MessageWriteError::MAVLink2Only);
1981 }
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]).await?;
1989
1990 Ok(len)
1991}
1992
1993#[cfg(feature = "embedded")]
1998pub async fn write_v1_msg_async<M: Message>(
1999 w: &mut impl embedded_io_async::Write,
2000 header: MavHeader,
2001 data: &M,
2002) -> Result<usize, error::MessageWriteError> {
2003 if data.message_id() > u8::MAX.into() {
2004 return Err(MessageWriteError::MAVLink2Only);
2005 }
2006 let mut message_raw = MAVLinkV1MessageRaw::new();
2007 message_raw.serialize_message(header, data);
2008
2009 let payload_length: usize = message_raw.payload_length().into();
2010 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
2011
2012 w.write_all(&message_raw.0[..len])
2013 .await
2014 .map_err(|_| error::MessageWriteError::Io)?;
2015
2016 Ok(len)
2017}