mavlink_core/
lib.rs

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