1#![cfg_attr(not(feature = "std"), no_std)]
23#![cfg_attr(all(any(docsrs, doc), not(doctest)), 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#[cfg(any(feature = "std", feature = "tokio-1"))]
86pub use connectable::{
87 ConnectionAddress, FileConnectable, SerialConnectable, TcpConnectable, UdpConnectable, UdpMode,
88};
89
90pub const MAX_FRAME_SIZE: usize = 280;
94
95pub trait Message
100where
101 Self: Sized,
102{
103 fn message_id(&self) -> u32;
105
106 fn message_name(&self) -> &'static str;
108
109 fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize;
111
112 fn parse(
114 version: MavlinkVersion,
115 msgid: u32,
116 payload: &[u8],
117 ) -> Result<Self, error::ParserError>;
118
119 fn message_id_from_name(name: &str) -> Result<u32, &'static str>;
121 fn default_message_from_id(id: u32) -> Result<Self, &'static str>;
123 #[cfg(feature = "arbitrary")]
125 fn random_message_from_id<R: rand::RngCore>(id: u32, rng: &mut R)
126 -> Result<Self, &'static str>;
127 fn extra_crc(id: u32) -> u8;
129}
130
131pub trait MessageData: Sized {
132 type Message: Message;
133
134 const ID: u32;
135 const NAME: &'static str;
136 const EXTRA_CRC: u8;
137 const ENCODED_LEN: usize;
138
139 fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize;
140 fn deser(version: MavlinkVersion, payload: &[u8]) -> Result<Self, ParserError>;
141}
142
143#[derive(Debug, Copy, Clone, PartialEq, Eq)]
145#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
146#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
147pub struct MavHeader {
148 pub system_id: u8,
150 pub component_id: u8,
152 pub sequence: u8,
154}
155
156#[derive(Debug, Copy, Clone, PartialEq, Eq)]
158#[cfg_attr(feature = "serde", derive(Serialize))]
159#[cfg_attr(feature = "serde", serde(tag = "type"))]
160#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
161pub enum MavlinkVersion {
162 V1,
164 V2,
166}
167
168pub const MAV_STX: u8 = 0xFE;
170
171pub const MAV_STX_V2: u8 = 0xFD;
173
174impl Default for MavHeader {
177 fn default() -> Self {
178 Self {
179 system_id: 255,
180 component_id: 0,
181 sequence: 0,
182 }
183 }
184}
185
186#[derive(Debug, Clone)]
190#[cfg_attr(feature = "serde", derive(Serialize))]
191#[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
192pub struct MavFrame<M: Message> {
193 pub header: MavHeader,
195 pub msg: M,
197 pub protocol_version: MavlinkVersion,
199}
200
201impl<M: Message> MavFrame<M> {
202 pub fn ser(&self, buf: &mut [u8]) -> usize {
206 let mut buf = bytes_mut::BytesMut::new(buf);
207
208 let mut payload_buf = [0u8; 255];
210 let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
211
212 buf.put_u8(self.header.sequence);
231 buf.put_u8(self.header.system_id);
232 buf.put_u8(self.header.component_id);
233
234 match self.protocol_version {
236 MavlinkVersion::V2 => {
237 let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
238 buf.put_slice(&bytes[..3]);
239 }
240 MavlinkVersion::V1 => {
241 buf.put_u8(self.msg.message_id() as u8); }
243 }
244
245 buf.put_slice(&payload_buf[..payload_len]);
246 buf.len()
247 }
248
249 pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result<Self, ParserError> {
253 let mut buf = Bytes::new(input);
254
255 let sequence = buf.get_u8();
265 let system_id = buf.get_u8();
266 let component_id = buf.get_u8();
267 let header = MavHeader {
268 system_id,
269 component_id,
270 sequence,
271 };
272
273 let msg_id = match version {
274 MavlinkVersion::V2 => buf.get_u24_le(),
275 MavlinkVersion::V1 => buf.get_u8().into(),
276 };
277
278 M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self {
279 header,
280 msg,
281 protocol_version: version,
282 })
283 }
284
285 pub fn header(&self) -> MavHeader {
287 self.header
288 }
289}
290
291pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 {
293 let mut crc_calculator = CRCu16::crc16mcrf4cc();
294 crc_calculator.digest(data);
295
296 crc_calculator.digest(&[extra_crc]);
297 crc_calculator.get_crc()
298}
299
300#[derive(Debug, Clone, Copy, PartialEq, Eq)]
301pub enum ReadVersion {
303 Single(MavlinkVersion),
305 Any,
307}
308
309impl ReadVersion {
310 #[cfg(feature = "std")]
311 fn from_conn_cfg<C: MavConnection<M>, M: Message>(conn: &C) -> Self {
312 if conn.allow_recv_any_version() {
313 Self::Any
314 } else {
315 conn.protocol_version().into()
316 }
317 }
318 #[cfg(feature = "tokio-1")]
319 fn from_async_conn_cfg<C: AsyncMavConnection<M>, M: Message + Sync + Send>(conn: &C) -> Self {
320 if conn.allow_recv_any_version() {
321 Self::Any
322 } else {
323 conn.protocol_version().into()
324 }
325 }
326}
327
328impl From<MavlinkVersion> for ReadVersion {
329 fn from(value: MavlinkVersion) -> Self {
330 Self::Single(value)
331 }
332}
333
334pub fn read_versioned_msg<M: Message, R: Read>(
336 r: &mut PeekReader<R>,
337 version: ReadVersion,
338) -> Result<(MavHeader, M), error::MessageReadError> {
339 match version {
340 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg(r),
341 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
342 ReadVersion::Any => read_any_msg(r),
343 }
344}
345
346#[cfg(feature = "tokio-1")]
348pub async fn read_versioned_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
349 r: &mut AsyncPeekReader<R>,
350 version: ReadVersion,
351) -> Result<(MavHeader, M), error::MessageReadError> {
352 match version {
353 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async(r).await,
354 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
355 ReadVersion::Any => read_any_msg_async(r).await,
356 }
357}
358
359#[cfg(feature = "signing")]
364pub fn read_versioned_msg_signed<M: Message, R: Read>(
365 r: &mut PeekReader<R>,
366 version: ReadVersion,
367 signing_data: Option<&SigningData>,
368) -> Result<(MavHeader, M), error::MessageReadError> {
369 match version {
370 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_inner(r, signing_data),
371 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg(r),
372 ReadVersion::Any => read_any_msg_inner(r, signing_data),
373 }
374}
375
376#[cfg(all(feature = "tokio-1", feature = "signing"))]
381pub async fn read_versioned_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
382 r: &mut AsyncPeekReader<R>,
383 version: ReadVersion,
384 signing_data: Option<&SigningData>,
385) -> Result<(MavHeader, M), error::MessageReadError> {
386 match version {
387 ReadVersion::Single(MavlinkVersion::V2) => read_v2_msg_async_inner(r, signing_data).await,
388 ReadVersion::Single(MavlinkVersion::V1) => read_v1_msg_async(r).await,
389 ReadVersion::Any => read_any_msg_async_inner(r, signing_data).await,
390 }
391}
392
393#[derive(Debug, Copy, Clone, PartialEq, Eq)]
394pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]);
399
400impl Default for MAVLinkV1MessageRaw {
401 fn default() -> Self {
402 Self::new()
403 }
404}
405
406impl MAVLinkV1MessageRaw {
407 const HEADER_SIZE: usize = 5;
408
409 pub const fn new() -> Self {
411 Self([0; 1 + Self::HEADER_SIZE + 255 + 2])
412 }
413
414 #[inline]
416 pub fn header(&self) -> &[u8] {
417 &self.0[1..=Self::HEADER_SIZE]
418 }
419
420 #[inline]
422 fn mut_header(&mut self) -> &mut [u8] {
423 &mut self.0[1..=Self::HEADER_SIZE]
424 }
425
426 #[inline]
428 pub fn payload_length(&self) -> u8 {
429 self.0[1]
430 }
431
432 #[inline]
434 pub fn sequence(&self) -> u8 {
435 self.0[2]
436 }
437
438 #[inline]
440 pub fn system_id(&self) -> u8 {
441 self.0[3]
442 }
443
444 #[inline]
446 pub fn component_id(&self) -> u8 {
447 self.0[4]
448 }
449
450 #[inline]
452 pub fn message_id(&self) -> u8 {
453 self.0[5]
454 }
455
456 #[inline]
458 pub fn payload(&self) -> &[u8] {
459 let payload_length: usize = self.payload_length().into();
460 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
461 }
462
463 #[inline]
465 pub fn checksum(&self) -> u16 {
466 let payload_length: usize = self.payload_length().into();
467 u16::from_le_bytes([
468 self.0[1 + Self::HEADER_SIZE + payload_length],
469 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
470 ])
471 }
472
473 #[inline]
474 fn mut_payload_and_checksum(&mut self) -> &mut [u8] {
475 let payload_length: usize = self.payload_length().into();
476 &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)]
477 }
478
479 #[inline]
481 pub fn has_valid_crc<M: Message>(&self) -> bool {
482 let payload_length: usize = self.payload_length().into();
483 self.checksum()
484 == calculate_crc(
485 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
486 M::extra_crc(self.message_id().into()),
487 )
488 }
489
490 pub fn raw_bytes(&self) -> &[u8] {
492 let payload_length = self.payload_length() as usize;
493 &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)]
494 }
495
496 fn serialize_stx_and_header_and_crc(
497 &mut self,
498 header: MavHeader,
499 msgid: u32,
500 payload_length: usize,
501 extra_crc: u8,
502 ) {
503 self.0[0] = MAV_STX;
504
505 let header_buf = self.mut_header();
506 header_buf.copy_from_slice(&[
507 payload_length as u8,
508 header.sequence,
509 header.system_id,
510 header.component_id,
511 msgid as u8,
512 ]);
513
514 let crc = calculate_crc(
515 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
516 extra_crc,
517 );
518 self.0[(1 + Self::HEADER_SIZE + payload_length)
519 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
520 .copy_from_slice(&crc.to_le_bytes());
521 }
522
523 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
525 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
526 let payload_length = message.ser(MavlinkVersion::V1, payload_buf);
527
528 let message_id = message.message_id();
529 self.serialize_stx_and_header_and_crc(
530 header,
531 message_id,
532 payload_length,
533 M::extra_crc(message_id),
534 );
535 }
536
537 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
538 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
539 let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf);
540
541 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC);
542 }
543}
544
545fn try_decode_v1<M: Message, R: Read>(
546 reader: &mut PeekReader<R>,
547) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
548 let mut message = MAVLinkV1MessageRaw::new();
549 let whole_header_size = MAVLinkV1MessageRaw::HEADER_SIZE + 1;
550
551 message.0[0] = MAV_STX;
552 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
553 message.mut_header().copy_from_slice(header);
554 let packet_length = message.raw_bytes().len();
555 let payload_and_checksum = &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
556 message
557 .mut_payload_and_checksum()
558 .copy_from_slice(payload_and_checksum);
559
560 if message.has_valid_crc::<M>() {
563 reader.consume(message.raw_bytes().len());
564 Ok(Some(message))
565 } else {
566 Ok(None)
567 }
568}
569
570#[cfg(feature = "tokio-1")]
571async fn try_decode_v1_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
573 reader: &mut AsyncPeekReader<R>,
574) -> Result<Option<MAVLinkV1MessageRaw>, error::MessageReadError> {
575 let mut message = MAVLinkV1MessageRaw::new();
576
577 message.0[0] = MAV_STX;
578 let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await?
579 [..MAVLinkV1MessageRaw::HEADER_SIZE];
580 message.mut_header().copy_from_slice(header);
581 let packet_length = message.raw_bytes().len() - 1;
582 let payload_and_checksum =
583 &reader.peek_exact(packet_length).await?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length];
584 message
585 .mut_payload_and_checksum()
586 .copy_from_slice(payload_and_checksum);
587
588 if message.has_valid_crc::<M>() {
591 reader.consume(message.raw_bytes().len() - 1);
592 Ok(Some(message))
593 } else {
594 Ok(None)
595 }
596}
597
598pub fn read_v1_raw_message<M: Message, R: Read>(
600 reader: &mut PeekReader<R>,
601) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
602 loop {
603 while reader.peek_exact(1)?[0] != MAV_STX {
605 reader.consume(1);
606 }
607
608 if let Some(msg) = try_decode_v1::<M, _>(reader)? {
609 return Ok(msg);
610 };
611
612 reader.consume(1);
613 }
614}
615
616#[cfg(feature = "tokio-1")]
618pub async fn read_v1_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
619 reader: &mut AsyncPeekReader<R>,
620) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
621 loop {
622 loop {
623 if reader.read_u8().await? == MAV_STX {
625 break;
626 }
627 }
628
629 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
630 return Ok(message);
631 }
632 }
633}
634
635#[cfg(feature = "embedded")]
641pub async fn read_v1_raw_message_async<M: Message>(
642 reader: &mut impl embedded_io_async::Read,
643) -> Result<MAVLinkV1MessageRaw, error::MessageReadError> {
644 loop {
645 let mut byte = [0u8];
647 loop {
648 reader
649 .read_exact(&mut byte)
650 .await
651 .map_err(|_| error::MessageReadError::Io)?;
652 if byte[0] == MAV_STX {
653 break;
654 }
655 }
656
657 let mut message = MAVLinkV1MessageRaw::new();
658
659 message.0[0] = MAV_STX;
660 reader
661 .read_exact(message.mut_header())
662 .await
663 .map_err(|_| error::MessageReadError::Io)?;
664 reader
665 .read_exact(message.mut_payload_and_checksum())
666 .await
667 .map_err(|_| error::MessageReadError::Io)?;
668
669 if message.has_valid_crc::<M>() {
672 return Ok(message);
673 }
674 }
675}
676
677pub fn read_v1_msg<M: Message, R: Read>(
679 r: &mut PeekReader<R>,
680) -> Result<(MavHeader, M), error::MessageReadError> {
681 let message = read_v1_raw_message::<M, _>(r)?;
682
683 Ok((
684 MavHeader {
685 sequence: message.sequence(),
686 system_id: message.system_id(),
687 component_id: message.component_id(),
688 },
689 M::parse(
690 MavlinkVersion::V1,
691 u32::from(message.message_id()),
692 message.payload(),
693 )?,
694 ))
695}
696
697#[cfg(feature = "tokio-1")]
699pub async fn read_v1_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
700 r: &mut AsyncPeekReader<R>,
701) -> Result<(MavHeader, M), error::MessageReadError> {
702 let message = read_v1_raw_message_async::<M, _>(r).await?;
703
704 Ok((
705 MavHeader {
706 sequence: message.sequence(),
707 system_id: message.system_id(),
708 component_id: message.component_id(),
709 },
710 M::parse(
711 MavlinkVersion::V1,
712 u32::from(message.message_id()),
713 message.payload(),
714 )?,
715 ))
716}
717
718#[cfg(feature = "embedded")]
723pub async fn read_v1_msg_async<M: Message>(
724 r: &mut impl embedded_io_async::Read,
725) -> Result<(MavHeader, M), error::MessageReadError> {
726 let message = read_v1_raw_message_async::<M>(r).await?;
727
728 Ok((
729 MavHeader {
730 sequence: message.sequence(),
731 system_id: message.system_id(),
732 component_id: message.component_id(),
733 },
734 M::parse(
735 MavlinkVersion::V1,
736 u32::from(message.message_id()),
737 message.payload(),
738 )?,
739 ))
740}
741
742const MAVLINK_IFLAG_SIGNED: u8 = 0x01;
743const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED;
744
745#[derive(Debug, Copy, Clone, PartialEq, Eq)]
746pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]);
751
752impl Default for MAVLinkV2MessageRaw {
753 fn default() -> Self {
754 Self::new()
755 }
756}
757
758impl MAVLinkV2MessageRaw {
759 const HEADER_SIZE: usize = 9;
760 const SIGNATURE_SIZE: usize = 13;
761
762 pub const fn new() -> Self {
764 Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE])
765 }
766
767 #[inline]
769 pub fn header(&self) -> &[u8] {
770 &self.0[1..=Self::HEADER_SIZE]
771 }
772
773 #[inline]
775 fn mut_header(&mut self) -> &mut [u8] {
776 &mut self.0[1..=Self::HEADER_SIZE]
777 }
778
779 #[inline]
781 pub fn payload_length(&self) -> u8 {
782 self.0[1]
783 }
784
785 #[inline]
789 pub fn incompatibility_flags(&self) -> u8 {
790 self.0[2]
791 }
792
793 #[inline]
797 pub fn incompatibility_flags_mut(&mut self) -> &mut u8 {
798 &mut self.0[2]
799 }
800
801 #[inline]
803 pub fn compatibility_flags(&self) -> u8 {
804 self.0[3]
805 }
806
807 #[inline]
809 pub fn sequence(&self) -> u8 {
810 self.0[4]
811 }
812
813 #[inline]
815 pub fn system_id(&self) -> u8 {
816 self.0[5]
817 }
818
819 #[inline]
821 pub fn component_id(&self) -> u8 {
822 self.0[6]
823 }
824
825 #[inline]
827 pub fn message_id(&self) -> u32 {
828 u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0])
829 }
830
831 #[inline]
833 pub fn payload(&self) -> &[u8] {
834 let payload_length: usize = self.payload_length().into();
835 &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)]
836 }
837
838 #[inline]
840 pub fn checksum(&self) -> u16 {
841 let payload_length: usize = self.payload_length().into();
842 u16::from_le_bytes([
843 self.0[1 + Self::HEADER_SIZE + payload_length],
844 self.0[1 + Self::HEADER_SIZE + payload_length + 1],
845 ])
846 }
847
848 #[cfg(feature = "signing")]
850 #[inline]
851 pub fn checksum_bytes(&self) -> &[u8] {
852 let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize;
853 &self.0[checksum_offset..(checksum_offset + 2)]
854 }
855
856 #[cfg(feature = "signing")]
860 #[inline]
861 pub fn signature_link_id(&self) -> u8 {
862 let payload_length: usize = self.payload_length().into();
863 self.0[1 + Self::HEADER_SIZE + payload_length + 2]
864 }
865
866 #[cfg(feature = "signing")]
868 #[inline]
869 pub fn signature_link_id_mut(&mut self) -> &mut u8 {
870 let payload_length: usize = self.payload_length().into();
871 &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2]
872 }
873
874 #[cfg(feature = "signing")]
880 #[inline]
881 pub fn signature_timestamp(&self) -> u64 {
882 let mut timestamp_bytes = [0u8; 8];
883 timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes());
884 u64::from_le_bytes(timestamp_bytes)
885 }
886
887 #[cfg(feature = "signing")]
891 #[inline]
892 pub fn signature_timestamp_bytes(&self) -> &[u8] {
893 let payload_length: usize = self.payload_length().into();
894 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
895 &self.0[timestamp_start..(timestamp_start + 6)]
896 }
897
898 #[cfg(feature = "signing")]
900 #[inline]
901 pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] {
902 let payload_length: usize = self.payload_length().into();
903 let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3;
904 &mut self.0[timestamp_start..(timestamp_start + 6)]
905 }
906
907 #[cfg(feature = "signing")]
911 #[inline]
912 pub fn signature_value(&self) -> &[u8] {
913 let payload_length: usize = self.payload_length().into();
914 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
915 &self.0[signature_start..(signature_start + 6)]
916 }
917
918 #[cfg(feature = "signing")]
920 #[inline]
921 pub fn signature_value_mut(&mut self) -> &mut [u8] {
922 let payload_length: usize = self.payload_length().into();
923 let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6;
924 &mut self.0[signature_start..(signature_start + 6)]
925 }
926
927 fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] {
928 let payload_length: usize = self.payload_length().into();
929
930 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
932 0
933 } else {
934 Self::SIGNATURE_SIZE
935 };
936
937 &mut self.0
938 [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
939 }
940
941 #[inline]
943 pub fn has_valid_crc<M: Message>(&self) -> bool {
944 let payload_length: usize = self.payload_length().into();
945 self.checksum()
946 == calculate_crc(
947 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
948 M::extra_crc(self.message_id()),
949 )
950 }
951
952 #[cfg(feature = "signing")]
956 pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) {
957 let mut hasher = Sha256::new();
958 hasher.update(secret_key);
959 hasher.update([MAV_STX_V2]);
960 hasher.update(self.header());
961 hasher.update(self.payload());
962 hasher.update(self.checksum_bytes());
963 hasher.update([self.signature_link_id()]);
964 hasher.update(self.signature_timestamp_bytes());
965 target_buffer.copy_from_slice(&hasher.finalize()[0..6]);
966 }
967
968 pub fn raw_bytes(&self) -> &[u8] {
970 let payload_length = self.payload_length() as usize;
971
972 let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 {
973 0
974 } else {
975 Self::SIGNATURE_SIZE
976 };
977
978 &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)]
979 }
980
981 fn serialize_stx_and_header_and_crc(
982 &mut self,
983 header: MavHeader,
984 msgid: u32,
985 payload_length: usize,
986 extra_crc: u8,
987 incompat_flags: u8,
988 ) {
989 self.0[0] = MAV_STX_V2;
990 let msgid_bytes = msgid.to_le_bytes();
991
992 let header_buf = self.mut_header();
993 header_buf.copy_from_slice(&[
994 payload_length as u8,
995 incompat_flags,
996 0, header.sequence,
998 header.system_id,
999 header.component_id,
1000 msgid_bytes[0],
1001 msgid_bytes[1],
1002 msgid_bytes[2],
1003 ]);
1004
1005 let crc = calculate_crc(
1006 &self.0[1..(1 + Self::HEADER_SIZE + payload_length)],
1007 extra_crc,
1008 );
1009 self.0[(1 + Self::HEADER_SIZE + payload_length)
1010 ..(1 + Self::HEADER_SIZE + payload_length + 2)]
1011 .copy_from_slice(&crc.to_le_bytes());
1012 }
1013
1014 pub fn serialize_message<M: Message>(&mut self, header: MavHeader, message: &M) {
1018 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1019 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1020
1021 let message_id = message.message_id();
1022 self.serialize_stx_and_header_and_crc(
1023 header,
1024 message_id,
1025 payload_length,
1026 M::extra_crc(message_id),
1027 0,
1028 );
1029 }
1030
1031 pub fn serialize_message_for_signing<M: Message>(&mut self, header: MavHeader, message: &M) {
1036 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1037 let payload_length = message.ser(MavlinkVersion::V2, payload_buf);
1038
1039 let message_id = message.message_id();
1040 self.serialize_stx_and_header_and_crc(
1041 header,
1042 message_id,
1043 payload_length,
1044 M::extra_crc(message_id),
1045 MAVLINK_IFLAG_SIGNED,
1046 );
1047 }
1048
1049 pub fn serialize_message_data<D: MessageData>(&mut self, header: MavHeader, message_data: &D) {
1050 let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)];
1051 let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf);
1052
1053 self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0);
1054 }
1055}
1056
1057#[allow(unused_variables)]
1058fn try_decode_v2<M: Message, R: Read>(
1059 reader: &mut PeekReader<R>,
1060 signing_data: Option<&SigningData>,
1061) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1062 let mut message = MAVLinkV2MessageRaw::new();
1063 let whole_header_size = MAVLinkV2MessageRaw::HEADER_SIZE + 1;
1064
1065 message.0[0] = MAV_STX_V2;
1066 let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
1067 message.mut_header().copy_from_slice(header);
1068
1069 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1070 reader.consume(1);
1072 return Ok(None);
1073 }
1074
1075 let packet_length = message.raw_bytes().len();
1076 let payload_and_checksum_and_sign =
1077 &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
1078 message
1079 .mut_payload_and_checksum_and_sign()
1080 .copy_from_slice(payload_and_checksum_and_sign);
1081
1082 if message.has_valid_crc::<M>() {
1083 reader.consume(message.raw_bytes().len());
1085 } else {
1086 reader.consume(1);
1087 return Ok(None);
1088 }
1089
1090 #[cfg(feature = "signing")]
1091 if let Some(signing_data) = signing_data {
1092 if !signing_data.verify_signature(&message) {
1093 return Ok(None);
1094 }
1095 }
1096
1097 Ok(Some(message))
1098}
1099
1100#[cfg(feature = "tokio-1")]
1101#[allow(unused_variables)]
1102async fn try_decode_v2_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1104 reader: &mut AsyncPeekReader<R>,
1105 signing_data: Option<&SigningData>,
1106) -> Result<Option<MAVLinkV2MessageRaw>, error::MessageReadError> {
1107 let mut message = MAVLinkV2MessageRaw::new();
1108
1109 message.0[0] = MAV_STX_V2;
1110 let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE).await?
1111 [..MAVLinkV2MessageRaw::HEADER_SIZE];
1112 message.mut_header().copy_from_slice(header);
1113
1114 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1115 return Ok(None);
1117 }
1118
1119 let packet_length = message.raw_bytes().len() - 1;
1120 let payload_and_checksum_and_sign =
1121 &reader.peek_exact(packet_length).await?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length];
1122 message
1123 .mut_payload_and_checksum_and_sign()
1124 .copy_from_slice(payload_and_checksum_and_sign);
1125
1126 if message.has_valid_crc::<M>() {
1127 reader.consume(message.raw_bytes().len() - 1);
1129 } else {
1130 return Ok(None);
1131 }
1132
1133 #[cfg(feature = "signing")]
1134 if let Some(signing_data) = signing_data {
1135 if !signing_data.verify_signature(&message) {
1136 return Ok(None);
1137 }
1138 }
1139
1140 Ok(Some(message))
1141}
1142
1143#[inline]
1145pub fn read_v2_raw_message<M: Message, R: Read>(
1146 reader: &mut PeekReader<R>,
1147) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1148 read_v2_raw_message_inner::<M, R>(reader, None)
1149}
1150
1151#[cfg(feature = "signing")]
1153#[inline]
1154pub fn read_v2_raw_message_signed<M: Message, R: Read>(
1155 reader: &mut PeekReader<R>,
1156 signing_data: Option<&SigningData>,
1157) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1158 read_v2_raw_message_inner::<M, R>(reader, signing_data)
1159}
1160
1161#[allow(unused_variables)]
1162fn read_v2_raw_message_inner<M: Message, R: Read>(
1163 reader: &mut PeekReader<R>,
1164 signing_data: Option<&SigningData>,
1165) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1166 loop {
1167 while reader.peek_exact(1)?[0] != MAV_STX_V2 {
1169 reader.consume(1);
1170 }
1171
1172 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1173 return Ok(message);
1174 }
1175 }
1176}
1177
1178#[cfg(feature = "tokio-1")]
1180pub async fn read_v2_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1181 reader: &mut AsyncPeekReader<R>,
1182) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1183 read_v2_raw_message_async_inner::<M, R>(reader, None).await
1184}
1185
1186#[cfg(feature = "tokio-1")]
1187#[allow(unused_variables)]
1188async fn read_v2_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1189 reader: &mut AsyncPeekReader<R>,
1190 signing_data: Option<&SigningData>,
1191) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1192 loop {
1193 loop {
1194 if reader.read_u8().await? == MAV_STX_V2 {
1196 break;
1197 }
1198 }
1199
1200 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1201 return Ok(message);
1202 }
1203 }
1204}
1205
1206#[cfg(all(feature = "tokio-1", feature = "signing"))]
1208pub async fn read_v2_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1209 reader: &mut AsyncPeekReader<R>,
1210 signing_data: Option<&SigningData>,
1211) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1212 read_v2_raw_message_async_inner::<M, R>(reader, signing_data).await
1213}
1214
1215#[cfg(feature = "embedded")]
1220pub async fn read_v2_raw_message_async<M: Message>(
1221 reader: &mut impl embedded_io_async::Read,
1222) -> Result<MAVLinkV2MessageRaw, error::MessageReadError> {
1223 loop {
1224 let mut byte = [0u8];
1226 loop {
1227 reader
1228 .read_exact(&mut byte)
1229 .await
1230 .map_err(|_| error::MessageReadError::Io)?;
1231 if byte[0] == MAV_STX_V2 {
1232 break;
1233 }
1234 }
1235
1236 let mut message = MAVLinkV2MessageRaw::new();
1237
1238 message.0[0] = MAV_STX_V2;
1239 reader
1240 .read_exact(message.mut_header())
1241 .await
1242 .map_err(|_| error::MessageReadError::Io)?;
1243
1244 if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
1245 continue;
1247 }
1248
1249 reader
1250 .read_exact(message.mut_payload_and_checksum_and_sign())
1251 .await
1252 .map_err(|_| error::MessageReadError::Io)?;
1253
1254 if message.has_valid_crc::<M>() {
1257 return Ok(message);
1258 }
1259 }
1260}
1261
1262#[inline]
1264pub fn read_v2_msg<M: Message, R: Read>(
1265 read: &mut PeekReader<R>,
1266) -> Result<(MavHeader, M), error::MessageReadError> {
1267 read_v2_msg_inner(read, None)
1268}
1269
1270#[cfg(feature = "signing")]
1272#[inline]
1273pub fn read_v2_msg_signed<M: Message, R: Read>(
1274 read: &mut PeekReader<R>,
1275 signing_data: Option<&SigningData>,
1276) -> Result<(MavHeader, M), error::MessageReadError> {
1277 read_v2_msg_inner(read, signing_data)
1278}
1279
1280fn read_v2_msg_inner<M: Message, R: Read>(
1281 read: &mut PeekReader<R>,
1282 signing_data: Option<&SigningData>,
1283) -> Result<(MavHeader, M), error::MessageReadError> {
1284 let message = read_v2_raw_message_inner::<M, _>(read, signing_data)?;
1285
1286 Ok((
1287 MavHeader {
1288 sequence: message.sequence(),
1289 system_id: message.system_id(),
1290 component_id: message.component_id(),
1291 },
1292 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1293 ))
1294}
1295
1296#[cfg(feature = "tokio-1")]
1298pub async fn read_v2_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1299 read: &mut AsyncPeekReader<R>,
1300) -> Result<(MavHeader, M), error::MessageReadError> {
1301 read_v2_msg_async_inner(read, None).await
1302}
1303
1304#[cfg(all(feature = "tokio-1", feature = "signing"))]
1306pub async fn read_v2_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1307 read: &mut AsyncPeekReader<R>,
1308 signing_data: Option<&SigningData>,
1309) -> Result<(MavHeader, M), error::MessageReadError> {
1310 read_v2_msg_async_inner(read, signing_data).await
1311}
1312
1313#[cfg(feature = "tokio-1")]
1314async fn read_v2_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1315 read: &mut AsyncPeekReader<R>,
1316 signing_data: Option<&SigningData>,
1317) -> Result<(MavHeader, M), error::MessageReadError> {
1318 let message = read_v2_raw_message_async_inner::<M, _>(read, signing_data).await?;
1319
1320 Ok((
1321 MavHeader {
1322 sequence: message.sequence(),
1323 system_id: message.system_id(),
1324 component_id: message.component_id(),
1325 },
1326 M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?,
1327 ))
1328}
1329
1330#[cfg(feature = "embedded")]
1335pub async fn read_v2_msg_async<M: Message, R: embedded_io_async::Read>(
1336 r: &mut R,
1337) -> Result<(MavHeader, M), error::MessageReadError> {
1338 let message = read_v2_raw_message_async::<M>(r).await?;
1339
1340 Ok((
1341 MavHeader {
1342 sequence: message.sequence(),
1343 system_id: message.system_id(),
1344 component_id: message.component_id(),
1345 },
1346 M::parse(
1347 MavlinkVersion::V2,
1348 u32::from(message.message_id()),
1349 message.payload(),
1350 )?,
1351 ))
1352}
1353
1354pub enum MAVLinkMessageRaw {
1356 V1(MAVLinkV1MessageRaw),
1357 V2(MAVLinkV2MessageRaw),
1358}
1359
1360impl MAVLinkMessageRaw {
1361 fn payload(&self) -> &[u8] {
1362 match self {
1363 Self::V1(msg) => msg.payload(),
1364 Self::V2(msg) => msg.payload(),
1365 }
1366 }
1367 fn sequence(&self) -> u8 {
1368 match self {
1369 Self::V1(msg) => msg.sequence(),
1370 Self::V2(msg) => msg.sequence(),
1371 }
1372 }
1373 fn system_id(&self) -> u8 {
1374 match self {
1375 Self::V1(msg) => msg.system_id(),
1376 Self::V2(msg) => msg.system_id(),
1377 }
1378 }
1379 fn component_id(&self) -> u8 {
1380 match self {
1381 Self::V1(msg) => msg.component_id(),
1382 Self::V2(msg) => msg.component_id(),
1383 }
1384 }
1385 fn message_id(&self) -> u32 {
1386 match self {
1387 Self::V1(msg) => u32::from(msg.message_id()),
1388 Self::V2(msg) => msg.message_id(),
1389 }
1390 }
1391 fn version(&self) -> MavlinkVersion {
1392 match self {
1393 Self::V1(_) => MavlinkVersion::V1,
1394 Self::V2(_) => MavlinkVersion::V2,
1395 }
1396 }
1397}
1398
1399#[inline]
1401pub fn read_any_raw_message<M: Message, R: Read>(
1402 reader: &mut PeekReader<R>,
1403) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1404 read_any_raw_message_inner::<M, R>(reader, None)
1405}
1406
1407#[cfg(feature = "signing")]
1409#[inline]
1410pub fn read_any_raw_message_signed<M: Message, R: Read>(
1411 reader: &mut PeekReader<R>,
1412 signing_data: Option<&SigningData>,
1413) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1414 read_any_raw_message_inner::<M, R>(reader, signing_data)
1415}
1416
1417#[allow(unused_variables)]
1418fn read_any_raw_message_inner<M: Message, R: Read>(
1419 reader: &mut PeekReader<R>,
1420 signing_data: Option<&SigningData>,
1421) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1422 loop {
1423 let version = loop {
1425 let byte = reader.peek_exact(1)?[0];
1426 if byte == MAV_STX {
1427 break MavlinkVersion::V1;
1428 }
1429 if byte == MAV_STX_V2 {
1430 break MavlinkVersion::V2;
1431 }
1432 reader.consume(1);
1433 };
1434 match version {
1435 MavlinkVersion::V1 => {
1436 if let Some(message) = try_decode_v1::<M, _>(reader)? {
1437 #[cfg(feature = "signing")]
1439 if let Some(signing) = signing_data {
1440 if signing.config.allow_unsigned {
1441 return Ok(MAVLinkMessageRaw::V1(message));
1442 }
1443 } else {
1444 return Ok(MAVLinkMessageRaw::V1(message));
1445 }
1446 #[cfg(not(feature = "signing"))]
1447 return Ok(MAVLinkMessageRaw::V1(message));
1448 }
1449
1450 reader.consume(1);
1451 }
1452 MavlinkVersion::V2 => {
1453 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {
1454 return Ok(MAVLinkMessageRaw::V2(message));
1455 }
1456 }
1457 }
1458 }
1459}
1460
1461#[cfg(feature = "tokio-1")]
1463pub async fn read_any_raw_message_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1464 reader: &mut AsyncPeekReader<R>,
1465) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1466 read_any_raw_message_async_inner::<M, R>(reader, None).await
1467}
1468
1469#[cfg(all(feature = "tokio-1", feature = "signing"))]
1471pub async fn read_any_raw_message_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1472 reader: &mut AsyncPeekReader<R>,
1473 signing_data: Option<&SigningData>,
1474) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1475 read_any_raw_message_async_inner::<M, R>(reader, signing_data).await
1476}
1477
1478#[cfg(feature = "tokio-1")]
1479#[allow(unused_variables)]
1480async fn read_any_raw_message_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1481 reader: &mut AsyncPeekReader<R>,
1482 signing_data: Option<&SigningData>,
1483) -> Result<MAVLinkMessageRaw, error::MessageReadError> {
1484 loop {
1485 let version = loop {
1487 let read = reader.read_u8().await?;
1488 if read == MAV_STX {
1489 break MavlinkVersion::V1;
1490 }
1491 if read == MAV_STX_V2 {
1492 break MavlinkVersion::V2;
1493 }
1494 };
1495
1496 match version {
1497 MavlinkVersion::V1 => {
1498 if let Some(message) = try_decode_v1_async::<M, _>(reader).await? {
1499 #[cfg(feature = "signing")]
1501 if let Some(signing) = signing_data {
1502 if signing.config.allow_unsigned {
1503 return Ok(MAVLinkMessageRaw::V1(message));
1504 }
1505 } else {
1506 return Ok(MAVLinkMessageRaw::V1(message));
1507 }
1508 #[cfg(not(feature = "signing"))]
1509 return Ok(MAVLinkMessageRaw::V1(message));
1510 }
1511 }
1512 MavlinkVersion::V2 => {
1513 if let Some(message) = try_decode_v2_async::<M, _>(reader, signing_data).await? {
1514 return Ok(MAVLinkMessageRaw::V2(message));
1515 }
1516 }
1517 }
1518 }
1519}
1520
1521#[inline]
1523pub fn read_any_msg<M: Message, R: Read>(
1524 read: &mut PeekReader<R>,
1525) -> Result<(MavHeader, M), error::MessageReadError> {
1526 read_any_msg_inner(read, None)
1527}
1528
1529#[cfg(feature = "signing")]
1533#[inline]
1534pub fn read_any_msg_signed<M: Message, R: Read>(
1535 read: &mut PeekReader<R>,
1536 signing_data: Option<&SigningData>,
1537) -> Result<(MavHeader, M), error::MessageReadError> {
1538 read_any_msg_inner(read, signing_data)
1539}
1540
1541fn read_any_msg_inner<M: Message, R: Read>(
1542 read: &mut PeekReader<R>,
1543 signing_data: Option<&SigningData>,
1544) -> Result<(MavHeader, M), error::MessageReadError> {
1545 let message = read_any_raw_message_inner::<M, _>(read, signing_data)?;
1546 Ok((
1547 MavHeader {
1548 sequence: message.sequence(),
1549 system_id: message.system_id(),
1550 component_id: message.component_id(),
1551 },
1552 M::parse(message.version(), message.message_id(), message.payload())?,
1553 ))
1554}
1555
1556#[cfg(feature = "tokio-1")]
1558pub async fn read_any_msg_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
1559 read: &mut AsyncPeekReader<R>,
1560) -> Result<(MavHeader, M), error::MessageReadError> {
1561 read_any_msg_async_inner(read, None).await
1562}
1563
1564#[cfg(all(feature = "tokio-1", feature = "signing"))]
1568#[inline]
1569pub async fn read_any_msg_async_signed<M: Message, R: tokio::io::AsyncRead + Unpin>(
1570 read: &mut AsyncPeekReader<R>,
1571 signing_data: Option<&SigningData>,
1572) -> Result<(MavHeader, M), error::MessageReadError> {
1573 read_any_msg_async_inner(read, signing_data).await
1574}
1575
1576#[cfg(feature = "tokio-1")]
1577async fn read_any_msg_async_inner<M: Message, R: tokio::io::AsyncRead + Unpin>(
1578 read: &mut AsyncPeekReader<R>,
1579 signing_data: Option<&SigningData>,
1580) -> Result<(MavHeader, M), error::MessageReadError> {
1581 let message = read_any_raw_message_async_inner::<M, _>(read, signing_data).await?;
1582
1583 Ok((
1584 MavHeader {
1585 sequence: message.sequence(),
1586 system_id: message.system_id(),
1587 component_id: message.component_id(),
1588 },
1589 M::parse(message.version(), message.message_id(), message.payload())?,
1590 ))
1591}
1592
1593pub fn write_versioned_msg<M: Message, W: Write>(
1595 w: &mut W,
1596 version: MavlinkVersion,
1597 header: MavHeader,
1598 data: &M,
1599) -> Result<usize, error::MessageWriteError> {
1600 match version {
1601 MavlinkVersion::V2 => write_v2_msg(w, header, data),
1602 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1603 }
1604}
1605
1606#[cfg(feature = "signing")]
1610pub fn write_versioned_msg_signed<M: Message, W: Write>(
1611 w: &mut W,
1612 version: MavlinkVersion,
1613 header: MavHeader,
1614 data: &M,
1615 signing_data: Option<&SigningData>,
1616) -> Result<usize, error::MessageWriteError> {
1617 match version {
1618 MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data),
1619 MavlinkVersion::V1 => write_v1_msg(w, header, data),
1620 }
1621}
1622
1623#[cfg(feature = "tokio-1")]
1625pub async fn write_versioned_msg_async<M: Message, W: AsyncWrite + Unpin>(
1626 w: &mut W,
1627 version: MavlinkVersion,
1628 header: MavHeader,
1629 data: &M,
1630) -> Result<usize, error::MessageWriteError> {
1631 match version {
1632 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1633 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1634 }
1635}
1636
1637#[cfg(all(feature = "tokio-1", feature = "signing"))]
1641pub async fn write_versioned_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1642 w: &mut W,
1643 version: MavlinkVersion,
1644 header: MavHeader,
1645 data: &M,
1646 signing_data: Option<&SigningData>,
1647) -> Result<usize, error::MessageWriteError> {
1648 match version {
1649 MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await,
1650 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1651 }
1652}
1653
1654#[cfg(feature = "embedded")]
1659pub async fn write_versioned_msg_async<M: Message>(
1660 w: &mut impl embedded_io_async::Write,
1661 version: MavlinkVersion,
1662 header: MavHeader,
1663 data: &M,
1664) -> Result<usize, error::MessageWriteError> {
1665 match version {
1666 MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await,
1667 MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await,
1668 }
1669}
1670
1671pub fn write_v2_msg<M: Message, W: Write>(
1673 w: &mut W,
1674 header: MavHeader,
1675 data: &M,
1676) -> Result<usize, error::MessageWriteError> {
1677 let mut message_raw = MAVLinkV2MessageRaw::new();
1678 message_raw.serialize_message(header, data);
1679
1680 let payload_length: usize = message_raw.payload_length().into();
1681 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1682
1683 w.write_all(&message_raw.0[..len])?;
1684
1685 Ok(len)
1686}
1687
1688#[cfg(feature = "signing")]
1690pub fn write_v2_msg_signed<M: Message, W: Write>(
1691 w: &mut W,
1692 header: MavHeader,
1693 data: &M,
1694 signing_data: Option<&SigningData>,
1695) -> Result<usize, error::MessageWriteError> {
1696 let mut message_raw = MAVLinkV2MessageRaw::new();
1697
1698 let signature_len = if let Some(signing_data) = signing_data {
1699 if signing_data.config.sign_outgoing {
1700 message_raw.serialize_message_for_signing(header, data);
1701 signing_data.sign_message(&mut message_raw);
1702 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1703 } else {
1704 message_raw.serialize_message(header, data);
1705 0
1706 }
1707 } else {
1708 message_raw.serialize_message(header, data);
1709 0
1710 };
1711
1712 let payload_length: usize = message_raw.payload_length().into();
1713 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1714
1715 w.write_all(&message_raw.0[..len])?;
1716
1717 Ok(len)
1718}
1719
1720#[cfg(feature = "tokio-1")]
1722pub async fn write_v2_msg_async<M: Message, W: AsyncWrite + Unpin>(
1723 w: &mut W,
1724 header: MavHeader,
1725 data: &M,
1726) -> Result<usize, error::MessageWriteError> {
1727 let mut message_raw = MAVLinkV2MessageRaw::new();
1728 message_raw.serialize_message(header, data);
1729
1730 let payload_length: usize = message_raw.payload_length().into();
1731 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1732
1733 w.write_all(&message_raw.0[..len]).await?;
1734
1735 Ok(len)
1736}
1737
1738#[cfg(feature = "signing")]
1740#[cfg(feature = "tokio-1")]
1741pub async fn write_v2_msg_async_signed<M: Message, W: AsyncWrite + Unpin>(
1742 w: &mut W,
1743 header: MavHeader,
1744 data: &M,
1745 signing_data: Option<&SigningData>,
1746) -> Result<usize, error::MessageWriteError> {
1747 let mut message_raw = MAVLinkV2MessageRaw::new();
1748
1749 let signature_len = if let Some(signing_data) = signing_data {
1750 if signing_data.config.sign_outgoing {
1751 message_raw.serialize_message_for_signing(header, data);
1752 signing_data.sign_message(&mut message_raw);
1753 MAVLinkV2MessageRaw::SIGNATURE_SIZE
1754 } else {
1755 message_raw.serialize_message(header, data);
1756 0
1757 }
1758 } else {
1759 message_raw.serialize_message(header, data);
1760 0
1761 };
1762
1763 let payload_length: usize = message_raw.payload_length().into();
1764 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len;
1765
1766 w.write_all(&message_raw.0[..len]).await?;
1767
1768 Ok(len)
1769}
1770
1771#[cfg(feature = "embedded")]
1776pub async fn write_v2_msg_async<M: Message>(
1777 w: &mut impl embedded_io_async::Write,
1778 header: MavHeader,
1779 data: &M,
1780) -> Result<usize, error::MessageWriteError> {
1781 let mut message_raw = MAVLinkV2MessageRaw::new();
1782 message_raw.serialize_message(header, data);
1783
1784 let payload_length: usize = message_raw.payload_length().into();
1785 let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2;
1786
1787 w.write_all(&message_raw.0[..len])
1788 .await
1789 .map_err(|_| error::MessageWriteError::Io)?;
1790
1791 Ok(len)
1792}
1793
1794pub fn write_v1_msg<M: Message, W: Write>(
1796 w: &mut W,
1797 header: MavHeader,
1798 data: &M,
1799) -> Result<usize, error::MessageWriteError> {
1800 let mut message_raw = MAVLinkV1MessageRaw::new();
1801 message_raw.serialize_message(header, data);
1802
1803 let payload_length: usize = message_raw.payload_length().into();
1804 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1805
1806 w.write_all(&message_raw.0[..len])?;
1807
1808 Ok(len)
1809}
1810
1811#[cfg(feature = "tokio-1")]
1813pub async fn write_v1_msg_async<M: Message, W: AsyncWrite + Unpin>(
1814 w: &mut W,
1815 header: MavHeader,
1816 data: &M,
1817) -> Result<usize, error::MessageWriteError> {
1818 let mut message_raw = MAVLinkV1MessageRaw::new();
1819 message_raw.serialize_message(header, data);
1820
1821 let payload_length: usize = message_raw.payload_length().into();
1822 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1823
1824 w.write_all(&message_raw.0[..len]).await?;
1825
1826 Ok(len)
1827}
1828
1829#[cfg(feature = "embedded")]
1834pub async fn write_v1_msg_async<M: Message>(
1835 w: &mut impl embedded_io_async::Write,
1836 header: MavHeader,
1837 data: &M,
1838) -> Result<usize, error::MessageWriteError> {
1839 let mut message_raw = MAVLinkV1MessageRaw::new();
1840 message_raw.serialize_message(header, data);
1841
1842 let payload_length: usize = message_raw.payload_length().into();
1843 let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2;
1844
1845 w.write_all(&message_raw.0[..len])
1846 .await
1847 .map_err(|_| error::MessageWriteError::Io)?;
1848
1849 Ok(len)
1850}