mavio/protocol/
header.rs

1//! # MAVLink header
2//!
3//! This module contains an implementation for MAVLink packet header both for `MAVLink 1` and
4//! `MAVLink 2` protocol versions.
5
6use core::marker::PhantomData;
7use tbytes::{TBytesReader, TBytesReaderFor};
8
9use crate::consts::{
10    CHECKSUM_SIZE, HEADER_MAX_SIZE, HEADER_MIN_SIZE, HEADER_V1_SIZE, HEADER_V2_SIZE,
11    SIGNATURE_LENGTH,
12};
13use crate::error::VersionError;
14use crate::io::{AsyncRead, AsyncWrite, Read, Write};
15use crate::protocol::marker::{HasCompId, HasMsgId, HasPayloadLen, HasSysId, Sequenced, Unset};
16use crate::protocol::{
17    CompatFlags, ComponentId, HeaderBuilder, IncompatFlags, MavSTX, MaybeVersioned, PayloadLength,
18    Sequence, SystemId, Versioned, Versionless, V2,
19};
20use crate::protocol::{MavLinkVersion, MessageId};
21
22use crate::prelude::*;
23
24/// MAVLink frame header.
25///
26/// Header contains information relevant to for `MAVLink 1` and `MAVLink 2` packet formats.
27///
28/// # Versioned and versionless headers
29///
30/// In most cases, you are going to receive a [`Versionless`] header. However, if you want to work
31/// in the context of a specific MAVLink protocol version, you can convert a header into a
32/// [`Versioned`] variant by calling [`Header::try_into_versioned`].
33///
34/// You always can forget about the header's version by calling [`Header::into_versionless`].
35///
36/// # Links
37///
38///  * [MAVLink 1 packet format](https://mavlink.io/en/guide/serialization.html#v1_packet_format).
39///  * [MAVLink 2 packet format](https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format).
40#[derive(Clone, Copy, Debug, Default)]
41#[cfg_attr(feature = "specta", derive(specta::Type))]
42#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
43pub struct Header<V: MaybeVersioned> {
44    pub(super) version: MavLinkVersion,
45    pub(super) payload_length: PayloadLength,
46    pub(super) incompat_flags: IncompatFlags,
47    pub(super) compat_flags: CompatFlags,
48    pub(super) sequence: Sequence,
49    pub(super) system_id: SystemId,
50    pub(super) component_id: ComponentId,
51    pub(super) message_id: MessageId,
52    pub(super) _marker_version: PhantomData<V>,
53}
54
55impl<V: MaybeVersioned> Header<V> {
56    /// MAVLink protocol version.
57    ///
58    /// MAVLink version defined by the magic byte (STX).
59    ///
60    /// See [`MavSTX`].
61    #[inline]
62    pub fn version(&self) -> MavLinkVersion {
63        self.version
64    }
65
66    /// Payload length.
67    ///
68    /// Indicates length of the following `payload` section. This may be affected by payload truncation.
69    #[inline]
70    pub fn payload_length(&self) -> PayloadLength {
71        self.payload_length
72    }
73
74    /// Packet sequence number.
75    ///
76    /// Used to detect packet loss. Components increment value for each message sent.
77    #[inline]
78    pub fn sequence(&self) -> Sequence {
79        self.sequence
80    }
81
82    /// System `ID`.
83    ///
84    /// `ID` of the system (vehicle) sending the message. Used to differentiate systems on a network.
85    ///
86    /// > Note that broadcast address 0 may not be used in this field as it is an invalid source
87    /// > address.
88    #[inline]
89    pub fn system_id(&self) -> SystemId {
90        self.system_id
91    }
92
93    /// Component `ID`.
94    ///
95    /// `ID` of the component sending the message. Used to differentiate components in a system
96    /// (e.g., autopilot and a camera). Use appropriate values in
97    /// [MAV_COMPONENT](https://mavlink.io/en/messages/common.html#MAV_COMPONENT).
98    ///
99    /// > Note that the broadcast address `MAV_COMP_ID_ALL` may not be used in this field as it is
100    /// > an invalid source address.
101    #[inline]
102    pub fn component_id(&self) -> ComponentId {
103        self.component_id
104    }
105
106    /// Message `ID`.
107    ///
108    /// `ID` of MAVLink message. Defines how the payload will be encoded and decoded.
109    #[inline]
110    pub fn message_id(&self) -> MessageId {
111        self.message_id
112    }
113
114    /// Size of the header in bytes.
115    ///
116    /// Depends on the MAVLink protocol version.
117    pub fn size(&self) -> usize {
118        match self.version {
119            MavLinkVersion::V1 => HEADER_V1_SIZE,
120            MavLinkVersion::V2 => HEADER_V2_SIZE,
121        }
122    }
123
124    /// Returns `true` if frame body should contain signature.
125    ///
126    /// For `MAVLink 1` headers always returns `false`.
127    ///
128    /// For `MAVLink 2` it checks for [`IncompatFlags::MAVLINK_IFLAG_SIGNED`] (default is `false`).
129    ///
130    /// # Links
131    ///
132    /// * [Frame::signature](Frame::signature).
133    pub fn is_signed(&self) -> bool {
134        match self.version {
135            MavLinkVersion::V1 => false,
136            MavLinkVersion::V2 => self
137                .incompat_flags
138                .contains(IncompatFlags::MAVLINK_IFLAG_SIGNED),
139        }
140    }
141
142    /// Sets whether `MAVLink 2` frame body should contain signature.
143    ///
144    /// Sets `MAVLINK_IFLAG_SIGNED` for [`Self::incompat_flags`].
145    #[inline]
146    pub(super) fn set_is_signed(&mut self, flag: bool) {
147        self.incompat_flags
148            .set(IncompatFlags::MAVLINK_IFLAG_SIGNED, flag);
149    }
150
151    /// MAVLink frame body length.
152    ///
153    /// Calculates expected size in bytes for the frame body. Depends on MAVLink protocol version
154    /// and presence of signature (when [`IncompatFlags::MAVLINK_IFLAG_SIGNED`] incompatibility flag
155    /// is set).
156    ///
157    /// # Links
158    ///
159    /// * [`Frame::signature`](Frame::signature).
160    pub fn body_length(&self) -> usize {
161        match self.version {
162            MavLinkVersion::V1 => self.payload_length as usize + CHECKSUM_SIZE,
163            MavLinkVersion::V2 => {
164                if self.is_signed() {
165                    self.payload_length as usize + CHECKSUM_SIZE + SIGNATURE_LENGTH
166                } else {
167                    self.payload_length as usize + CHECKSUM_SIZE
168                }
169            }
170        }
171    }
172
173    /// Attempts to transform an existing header into its versioned form.
174    ///
175    /// This method never changes the internal MAVLink protocol version. It will return an error
176    /// if conversion is not possible.
177    pub fn try_into_versioned<Version: MaybeVersioned>(
178        self,
179    ) -> core::result::Result<Header<Version>, VersionError> {
180        Version::expect(self.version)?;
181
182        Ok(Header {
183            version: self.version,
184            payload_length: self.payload_length,
185            incompat_flags: self.incompat_flags,
186            compat_flags: self.compat_flags,
187            sequence: self.sequence,
188            system_id: self.system_id,
189            component_id: self.component_id,
190            message_id: self.message_id,
191            _marker_version: PhantomData,
192        })
193    }
194
195    /// Attempts to create a header with a specified version from the existing one.
196    ///
197    /// This method never changes the internal MAVLink protocol version. It will return an error
198    /// if conversion is not possible.
199    pub fn try_to_versioned<Version: MaybeVersioned>(
200        &self,
201    ) -> core::result::Result<Header<Version>, VersionError> {
202        self.clone().try_into_versioned()
203    }
204
205    /// Forget about the header's version transforming it into a [`Versionless`] variant.
206    pub fn into_versionless(self) -> Header<Versionless> {
207        Header {
208            version: self.version,
209            payload_length: self.payload_length,
210            incompat_flags: self.incompat_flags,
211            compat_flags: self.compat_flags,
212            sequence: self.sequence,
213            system_id: self.system_id,
214            component_id: self.component_id,
215            message_id: self.message_id,
216            _marker_version: PhantomData,
217        }
218    }
219
220    /// Create a [`Versionless`] header from the existing one.
221    pub fn to_versionless(&self) -> Header<Versionless> {
222        self.clone().into_versionless()
223    }
224
225    pub(super) fn send<E: Into<Error>, W: Write<E>>(
226        &self,
227        writer: &mut W,
228    ) -> core::result::Result<usize, E> {
229        writer.write_all(self.serialize().as_slice())?;
230        Ok(self.size())
231    }
232
233    pub(super) async fn send_async<E: Into<Error>, W: AsyncWrite<E>>(
234        &self,
235        writer: &mut W,
236    ) -> core::result::Result<usize, E> {
237        writer.write_all(self.serialize().as_slice()).await?;
238        Ok(self.size())
239    }
240
241    fn dump_bytes(&self, header_bytes: &mut HeaderBytes) {
242        match self.version {
243            MavLinkVersion::V1 => self.dump_v1_bytes(header_bytes),
244            MavLinkVersion::V2 => self.dump_v2_bytes(header_bytes),
245        };
246    }
247
248    fn dump_v1_bytes(&self, header_bytes: &mut HeaderBytes) {
249        header_bytes.buffer[0] = MavSTX::V1.into();
250        header_bytes.buffer[1] = self.payload_length;
251        header_bytes.buffer[2] = self.sequence;
252        header_bytes.buffer[3] = self.system_id;
253        header_bytes.buffer[4] = self.component_id;
254        header_bytes.buffer[5] = self.message_id.to_le_bytes()[0];
255    }
256
257    fn dump_v2_bytes(&self, header_bytes: &mut HeaderBytes) {
258        let message_id: [u8; 4] = self.message_id.to_le_bytes();
259
260        header_bytes.buffer[0] = MavSTX::V2.into();
261        header_bytes.buffer[1] = self.payload_length;
262        header_bytes.buffer[2] = self.incompat_flags.bits();
263        header_bytes.buffer[3] = self.compat_flags.bits();
264        header_bytes.buffer[4] = self.sequence;
265        header_bytes.buffer[5] = self.system_id;
266        header_bytes.buffer[6] = self.component_id;
267        header_bytes.buffer[7..10].copy_from_slice(&message_id[0..3]);
268    }
269}
270
271impl<V: MaybeVersioned> Header<V> {
272    pub(super) fn serialize(&self) -> HeaderBytes {
273        let mut header_bytes = HeaderBytes {
274            size: self.size(),
275            ..Default::default()
276        };
277        self.dump_bytes(&mut header_bytes);
278        header_bytes
279    }
280
281    pub(super) fn deserialize(buf: &[u8]) -> core::result::Result<Header<V>, FrameError> {
282        if buf.len() < HEADER_MIN_SIZE {
283            return Err(FrameError::InvalidHeader);
284        }
285
286        let header_length = match MavSTX::from(buf[0]) {
287            MavSTX::V1 => HEADER_V1_SIZE,
288            MavSTX::V2 => HEADER_V2_SIZE,
289            MavSTX::Unknown(_) => return Err(FrameError::InvalidHeader),
290        };
291
292        if buf.len() < header_length {
293            return Err(FrameError::InvalidHeader);
294        }
295
296        Ok(unsafe { Header::<V>::try_from_slice_unchecked(&buf[0..header_length]) })
297    }
298
299    pub(super) fn recv<E: Into<Error>, R: Read<E>>(
300        reader: &mut R,
301    ) -> core::result::Result<Header<V>, E> {
302        loop {
303            let mut buffer = [0u8; HEADER_MIN_SIZE];
304            reader.read_exact(&mut buffer)?;
305
306            if let Some(mut header_start) = HeaderStart::<V>::from_slice(&buffer) {
307                if !header_start.is_complete() {
308                    reader.read_exact(header_start.remaining_bytes_mut())?;
309                }
310                return Ok(unsafe {
311                    Header::<V>::try_from_slice_unchecked(header_start.as_slice_unchecked())
312                });
313            } else {
314                continue;
315            }
316        }
317    }
318
319    pub(super) async fn recv_async<E: Into<Error>, R: AsyncRead<E>>(
320        reader: &mut R,
321    ) -> core::result::Result<Header<V>, E> {
322        loop {
323            let mut buffer = [0u8; HEADER_MIN_SIZE];
324            reader.read_exact(&mut buffer).await?;
325
326            if let Some(mut header_start) = HeaderStart::<V>::from_slice(&buffer) {
327                if !header_start.is_complete() {
328                    reader
329                        .read_exact(header_start.remaining_bytes_mut())
330                        .await?;
331                }
332                return Ok(unsafe {
333                    Header::<V>::try_from_slice_unchecked(header_start.as_slice_unchecked())
334                });
335            } else {
336                continue;
337            }
338        }
339    }
340
341    // This function does not use unsafe Rust but may panic if the first byte is not STX or provided
342    // slice has invalid size.
343    unsafe fn try_from_slice_unchecked(bytes: &[u8]) -> Header<V> {
344        let reader = TBytesReader::from(bytes);
345
346        let magic: u8 = reader.read().unwrap();
347        let mavlink_version: MavLinkVersion = MavSTX::from(magic).to_mavlink_version().unwrap();
348        let payload_length: u8 = reader.read().unwrap();
349
350        let (incompat_flags, compat_flags) = if let MavLinkVersion::V2 = mavlink_version {
351            let incompat_flags = reader.read().unwrap();
352            let compat_flags = reader.read().unwrap();
353            (
354                IncompatFlags::from_bits_truncate(incompat_flags),
355                CompatFlags::from_bits_truncate(compat_flags),
356            )
357        } else {
358            (IncompatFlags::default(), CompatFlags::default())
359        };
360
361        let sequence: u8 = reader.read().unwrap();
362        let system_id: u8 = reader.read().unwrap();
363        let component_id: u8 = reader.read().unwrap();
364
365        let message_id: MessageId = match mavlink_version {
366            MavLinkVersion::V1 => {
367                let version: u8 = reader.read().unwrap();
368                version as MessageId
369            }
370            MavLinkVersion::V2 => {
371                let version_byte: [u8; 4] = [
372                    reader.read().unwrap(),
373                    reader.read().unwrap(),
374                    reader.read().unwrap(),
375                    0,
376                ];
377                MessageId::from_le_bytes(version_byte)
378            }
379        };
380
381        let mut header_bytes = [0u8; HEADER_MAX_SIZE];
382        header_bytes[0..bytes.len()].copy_from_slice(bytes);
383
384        Header {
385            version: mavlink_version,
386            payload_length,
387            incompat_flags,
388            compat_flags,
389            sequence,
390            system_id,
391            component_id,
392            message_id,
393            _marker_version: PhantomData,
394        }
395    }
396}
397
398impl<V: Versioned> Header<V> {
399    pub(super) fn to_builder(
400        &self,
401    ) -> HeaderBuilder<V, HasPayloadLen, Sequenced, HasSysId, HasCompId, HasMsgId> {
402        let mut incompat_flags = self.incompat_flags;
403        incompat_flags.set(IncompatFlags::MAVLINK_IFLAG_SIGNED, false);
404
405        HeaderBuilder {
406            mavlink_version: PhantomData,
407            payload_length: HasPayloadLen(self.payload_length),
408            incompat_flags: Some(incompat_flags),
409            compat_flags: Some(self.compat_flags),
410            sequence: Sequenced(self.sequence),
411            system_id: HasSysId(self.system_id),
412            component_id: HasCompId(self.component_id),
413            message_id: HasMsgId(self.message_id),
414        }
415    }
416}
417
418impl Header<V2> {
419    /// Incompatibility flags for `MAVLink 2` header.
420    ///
421    /// Flags that must be understood for MAVLink compatibility (implementation discards a packet if
422    /// it does not understand a flag).
423    ///
424    /// See: [MAVLink 2 incompatibility flags](https://mavlink.io/en/guide/serialization.html#incompat_flags).
425    #[inline]
426    pub fn incompat_flags(&self) -> IncompatFlags {
427        self.incompat_flags
428    }
429
430    /// Compatibility flags for `MAVLink 2` header.
431    ///
432    /// Flags that can be ignored if not understood (implementation can still handle a packet even
433    /// if it does not understand a flag).
434    ///
435    /// See: [MAVLink 2 compatibility flags](https://mavlink.io/en/guide/serialization.html#compat_flags).
436    #[inline]
437    pub fn compat_flags(&self) -> CompatFlags {
438        self.compat_flags
439    }
440}
441
442impl Header<Versionless> {
443    /// Initiates builder for [`Header`].
444    ///
445    /// Instead of constructor, we use
446    /// [builder](https://rust-unofficial.github.io/patterns/patterns/creational/builder.html)
447    /// pattern. An instance of [`HeaderBuilder`] returned by this function is initialized
448    /// with default values. Once desired values are set, you can call [`HeaderBuilder::build`]
449    /// to obtain [`Header`].
450    pub fn builder() -> HeaderBuilder<Versionless, Unset, Unset, Unset, Unset, Unset> {
451        HeaderBuilder::new()
452    }
453
454    /// Incompatibility flags for `MAVLink 2` header.
455    ///
456    /// Flags that must be understood for MAVLink compatibility (implementation discards a packet if
457    /// it does not understand a flag).
458    ///
459    /// See: [MAVLink 2 incompatibility flags](https://mavlink.io/en/guide/serialization.html#incompat_flags).
460    #[inline]
461    pub fn incompat_flags(&self) -> Option<IncompatFlags> {
462        match self.version() {
463            MavLinkVersion::V1 => None,
464            MavLinkVersion::V2 => Some(self.incompat_flags),
465        }
466    }
467
468    /// Compatibility flags for `MAVLink 2` header.
469    ///
470    /// Flags that can be ignored if not understood (implementation can still handle a packet even
471    /// if it does not understand a flag).
472    ///
473    /// See: [MAVLink 2 compatibility flags](https://mavlink.io/en/guide/serialization.html#compat_flags).
474    #[inline]
475    pub fn compat_flags(&self) -> Option<CompatFlags> {
476        match self.version() {
477            MavLinkVersion::V1 => None,
478            MavLinkVersion::V2 => Some(self.compat_flags),
479        }
480    }
481}
482
483/// Represents a potentially incomplete start of MAVLink header as a sequence of bytes.
484struct HeaderStart<V: MaybeVersioned> {
485    buffer: [u8; HEADER_MAX_SIZE],
486    n_bytes_read: usize,
487    n_bytes_left: usize,
488    _marker_version: PhantomData<V>,
489}
490
491impl<V: MaybeVersioned> HeaderStart<V> {
492    /// Read header byres sequence from a byte slice.
493    fn from_slice(buffer: &[u8]) -> Option<Self> {
494        let (mavlink_version, header_start_idx) = {
495            let mut mavlink_version: Option<MavLinkVersion> = None;
496            let mut header_start_idx = buffer.len();
497            for (i, &byte) in buffer.iter().enumerate() {
498                if V::is_magic_byte(byte) {
499                    header_start_idx = i;
500                    mavlink_version = MavSTX::from(byte).into();
501                    break;
502                }
503            }
504
505            (mavlink_version, header_start_idx)
506        };
507
508        match mavlink_version {
509            None => None,
510            Some(version) => {
511                let header_size = match version {
512                    MavLinkVersion::V1 => HEADER_V1_SIZE,
513                    MavLinkVersion::V2 => HEADER_V2_SIZE,
514                };
515
516                let n_bytes_to_read = core::cmp::min(buffer.len() - header_start_idx, header_size);
517                let header_start_bytes =
518                    &buffer[header_start_idx..header_start_idx + n_bytes_to_read];
519
520                let mut header_bytes = [0u8; HEADER_MAX_SIZE];
521                header_bytes[0..n_bytes_to_read].copy_from_slice(header_start_bytes);
522
523                Some(Self {
524                    buffer: header_bytes,
525                    n_bytes_read: n_bytes_to_read,
526                    n_bytes_left: header_size - n_bytes_to_read,
527                    _marker_version: PhantomData,
528                })
529            }
530        }
531    }
532
533    /// Header bytes as a slice.
534    ///
535    /// This function does not check the header sequence for completeness.
536    unsafe fn as_slice_unchecked(&self) -> &[u8] {
537        &self.buffer[0..self.n_bytes_read + self.n_bytes_left]
538    }
539
540    /// Returns `true` if header is complete.
541    fn is_complete(&self) -> bool {
542        self.n_bytes_left == 0
543    }
544
545    /// Mutable slice pointing to the remaining bytes that should be read to complete the header.
546    fn remaining_bytes_mut(&mut self) -> &mut [u8] {
547        &mut self.buffer[self.n_bytes_read..self.n_bytes_read + self.n_bytes_left]
548    }
549}
550
551/// Represents [`Header`] encoded as a sequence of bytes.
552#[derive(Clone, Copy, Debug, Default)]
553#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
554pub(super) struct HeaderBytes {
555    buffer: [u8; HEADER_MAX_SIZE],
556    size: usize,
557}
558
559impl HeaderBytes {
560    /// Encoded [`Header`] as a slice of bytes.
561    ///
562    /// The length of a slice matches [`Self::size`] and therefore [`Header::size`].
563    pub(super) fn as_slice(&self) -> &[u8] {
564        &self.buffer[0..self.size]
565    }
566
567    /// Size of the encoded [`Header`] in bytes.
568    pub(super) fn size(&self) -> usize {
569        self.size
570    }
571
572    /// Encoded [`Header`] CRC data.
573    ///
574    /// Returns all header data excluding `magic` byte.
575    ///
576    /// See:
577    ///  * [`MavLinkFrame::calculate_crc`](Frame::calculate_crc).
578    ///  * [MAVLink checksum](https://mavlink.io/en/guide/serialization.html#checksum) in MAVLink
579    ///    protocol documentation.
580    pub(super) fn crc_data(&self) -> &[u8] {
581        &self.buffer[1..self.size()]
582    }
583}
584
585#[cfg(test)]
586mod header_tests {
587    use super::*;
588
589    #[cfg(feature = "std")]
590    use std::io::Cursor;
591
592    use crate::consts::{STX_V1, STX_V2};
593
594    #[cfg(feature = "std")]
595    use crate::io::StdIoReader;
596
597    #[test]
598    #[cfg(feature = "std")]
599    fn read_v1_header() {
600        let buffer = vec![
601            12,     // \
602            24,     //  | Junk bytes
603            240,    // /
604            STX_V1, // magic byte
605            8,      // payload_length
606            1,      // sequence
607            10,     // system ID
608            255,    // component ID
609            0,      // message ID
610        ];
611        let mut reader = StdIoReader::new(Cursor::new(buffer));
612
613        let header = Header::<V1>::recv(&mut reader).unwrap();
614        let header = header.try_into_versioned::<V1>().unwrap();
615
616        assert!(header.try_into_versioned::<V2>().is_err());
617        assert!(matches!(header.version(), MavLinkVersion::V1));
618
619        assert_eq!(header.payload_length(), 8u8);
620        assert_eq!(header.sequence(), 1u8);
621        assert_eq!(header.system_id(), 10u8);
622        assert_eq!(header.component_id(), 255u8);
623        assert_eq!(header.message_id(), 0u32);
624    }
625
626    #[test]
627    #[cfg(feature = "std")]
628    fn read_v2_header() {
629        let buffer = vec![
630            12,     // \
631            24,     //  |Junk bytes
632            240,    // /
633            STX_V2, // magic byte
634            8,      // payload_length
635            1,      // incompatibility flags
636            0,      // compatibility flags
637            1,      // sequence
638            10,     // system ID
639            255,    // component ID
640            0,      // \
641            0,      //  | message ID
642            0,      // /
643        ];
644        let mut reader = StdIoReader::new(Cursor::new(buffer));
645
646        let header = Header::<Versionless>::recv(&mut reader).unwrap();
647        let header = header.try_into_versioned::<V2>().unwrap();
648
649        assert!(header.try_into_versioned::<V1>().is_err());
650        assert!(matches!(header.version(), MavLinkVersion::V2));
651
652        assert_eq!(header.payload_length(), 8u8);
653        assert_eq!(header.incompat_flags(), IncompatFlags::MAVLINK_IFLAG_SIGNED);
654        assert_eq!(header.compat_flags(), CompatFlags::default());
655        assert_eq!(header.sequence(), 1u8);
656        assert_eq!(header.system_id(), 10u8);
657        assert_eq!(header.component_id(), 255u8);
658        assert_eq!(header.message_id(), 0u32);
659    }
660
661    #[test]
662    #[cfg(feature = "std")]
663    fn read_v2_header_magic_bytes_in_sequence() {
664        let buffer = vec![
665            12,     // \
666            24,     //  |Junk bytes
667            240,    // /
668            STX_V2, // magic byte
669            8,      // payload_length
670            1,      // incompatibility flags
671            0,      // compatibility flags
672            STX_V2, // sequence
673            10,     // system ID
674            255,    // component ID
675            0,      // \
676            0,      //  | message ID
677            0,      // /
678        ];
679        let mut reader = StdIoReader::new(Cursor::new(buffer));
680
681        let header = Header::<V2>::recv(&mut reader).unwrap();
682
683        assert!(header.try_into_versioned::<V1>().is_err());
684        assert!(matches!(header.version(), MavLinkVersion::V2));
685
686        assert_eq!(header.payload_length(), 8u8);
687        assert_eq!(header.incompat_flags(), IncompatFlags::MAVLINK_IFLAG_SIGNED);
688        assert_eq!(header.compat_flags(), CompatFlags::default());
689        assert_eq!(header.sequence(), STX_V2);
690        assert_eq!(header.system_id(), 10u8);
691        assert_eq!(header.component_id(), 255u8);
692        assert_eq!(header.message_id(), 0u32);
693    }
694
695    #[test]
696    fn build_v1_header() {
697        let header = Header::builder()
698            .payload_length(10)
699            .sequence(5)
700            .system_id(10)
701            .component_id(240)
702            .message_id(42)
703            .version(V1)
704            .build();
705
706        assert!(matches!(header.version(), MavLinkVersion::V1));
707        assert_eq!(header.payload_length(), 10);
708        assert_eq!(header.sequence(), 5);
709        assert_eq!(header.system_id(), 10);
710        assert_eq!(header.component_id(), 240);
711        assert_eq!(header.message_id(), 42);
712    }
713
714    #[test]
715    fn build_v2_header() {
716        let header = Header::builder()
717            .incompat_flags(IncompatFlags::MAVLINK_IFLAG_SIGNED)
718            .compat_flags(CompatFlags::BIT_4 | CompatFlags::BIT_7)
719            .payload_length(10)
720            .sequence(5)
721            .system_id(10)
722            .component_id(240)
723            .message_id(42)
724            .signed(true)
725            .build();
726
727        assert!(matches!(header.version(), MavLinkVersion::V2));
728        assert_eq!(header.incompat_flags(), IncompatFlags::MAVLINK_IFLAG_SIGNED);
729        assert_eq!(
730            header.compat_flags(),
731            CompatFlags::BIT_4 | CompatFlags::BIT_7
732        );
733        assert_eq!(header.payload_length(), 10);
734        assert_eq!(header.sequence(), 5);
735        assert_eq!(header.system_id(), 10);
736        assert_eq!(header.component_id(), 240);
737        assert_eq!(header.message_id(), 42);
738    }
739
740    #[test]
741    fn serialize_deserialize_v1_test() {
742        let sequence = [
743            STX_V1, // magic byte
744            8,      // payload_length
745            1,      // sequence
746            10,     // system ID
747            255,    // component ID
748            42,     // message ID
749            41,     // \
750            41,     //  | Anything
751            41,     // /
752        ];
753        let header_bytes = &sequence[0..HEADER_V1_SIZE];
754
755        // Deserialize from the exact sequence
756        let header: Header<V1> = Header::deserialize(&header_bytes).unwrap();
757        assert_eq!(header.payload_length(), 8);
758        assert_eq!(header.component_id(), 255);
759        assert_eq!(header.message_id(), 42);
760
761        // Serialize header
762        let serialized = header.serialize();
763        assert_eq!(serialized.as_slice(), header_bytes);
764
765        // Deserialize from a sequence with extra bytes
766        let header: Header<V1> = Header::deserialize(&sequence).unwrap();
767        let serialized = header.serialize();
768        assert_eq!(serialized.as_slice(), header_bytes);
769
770        // Deserialize from an incomplete sequence
771        assert!(matches!(
772            Header::<V1>::deserialize(&header_bytes[0..header_bytes.len() - 2]),
773            Err(FrameError::InvalidHeader)
774        ));
775    }
776
777    #[test]
778    fn serialize_deserialize_v2_test() {
779        let sequence = [
780            STX_V2, // magic byte
781            8,      // payload_length
782            1,      // incompatibility flags
783            0,      // compatibility flags
784            42,     // sequence
785            10,     // system ID
786            255,    // component ID
787            2,      // \
788            1,      //  | message ID
789            0,      // /
790        ];
791        let header_bytes = &sequence[0..HEADER_V2_SIZE];
792
793        // Deserialize from the exact sequence
794        let header: Header<V2> = Header::deserialize(&header_bytes).unwrap();
795        assert_eq!(header.payload_length(), 8);
796        assert_eq!(header.component_id(), 255);
797        assert_eq!(header.message_id(), 258);
798
799        // Serialize header
800        let serialized = header.serialize();
801        assert_eq!(serialized.as_slice(), header_bytes);
802
803        // Deserialize from a sequence with extra bytes
804        let header: Header<V2> = Header::deserialize(&sequence).unwrap();
805        let serialized = header.serialize();
806        assert_eq!(serialized.as_slice(), header_bytes);
807
808        // Deserialize from an incomplete sequence
809        assert!(matches!(
810            Header::<V2>::deserialize(&header_bytes[0..header_bytes.len() - 2]),
811            Err(FrameError::InvalidHeader)
812        ));
813    }
814
815    #[test]
816    fn header_start_v2_test() {
817        let v2_sequence = [
818            STX_V2, // magic byte
819            8,      // payload_length
820            1,      // incompatibility flags
821            0,      // compatibility flags
822            42,     // sequence
823            10,     // system ID
824            255,    // component ID
825            2,      // \
826            1,      //  | message ID
827            0,      // /
828            41,     // \
829            41,     //  | Anything
830            41,     // /
831        ];
832
833        // Complete header sequence
834        let header_start = HeaderStart::<V2>::from_slice(&v2_sequence).unwrap();
835        assert!(header_start.is_complete());
836
837        let header: Header<V2> =
838            Header::deserialize(unsafe { header_start.as_slice_unchecked() }).unwrap();
839        assert_eq!(header.payload_length(), 8);
840        assert_eq!(header.component_id(), 255);
841        assert_eq!(header.message_id(), 258);
842
843        // Incomplete header sequence
844        let mut header_start = HeaderStart::<V2>::from_slice(&v2_sequence[0..5]).unwrap();
845        assert!(!header_start.is_complete());
846
847        let n_bytes_left = header_start.n_bytes_left;
848        header_start
849            .remaining_bytes_mut()
850            .copy_from_slice(&v2_sequence[5..5 + n_bytes_left]);
851
852        let header: Header<V2> =
853            Header::deserialize(unsafe { header_start.as_slice_unchecked() }).unwrap();
854        assert_eq!(header.payload_length(), 8);
855        assert_eq!(header.component_id(), 255);
856        assert_eq!(header.message_id(), 258);
857    }
858}
mavio/protocol/header.rs

mavio/protocol/
header.rs
