From 4faca0ec695c5598ed3421466f8c57139d77d5de Mon Sep 17 00:00:00 2001
From: "Alex M. Smith" <amsmith.pro@gmail.com>
Date: Wed, 7 Aug 2024 18:05:01 +0200
Subject: [PATCH] fix: add MavFrame deser & ser warning, fixes #250

---
 mavlink-core/src/lib.rs          | 39 ++++++++++++++++++++++++++++----
 mavlink/tests/mav_frame_tests.rs |  7 +++++-
 2 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs
index 342d729..5583793 100644
--- a/mavlink-core/src/lib.rs
+++ b/mavlink-core/src/lib.rs
@@ -127,7 +127,7 @@ impl Default for MavHeader {
 
 /// Encapsulation of the Mavlink message and the header,
 /// important to preserve information about the sender system
-/// and component id
+/// and component id.
 #[derive(Debug, Clone)]
 #[cfg_attr(feature = "serde", derive(Serialize))]
 pub struct MavFrame<M: Message> {
@@ -146,9 +146,32 @@ impl<M: Message> MavFrame<M> {
     //    }
 
     /// Serialize MavFrame into a vector, so it can be sent over a socket, for example.
+    /// The resulting buffer will start with the sequence field of the Mavlink frame
+    /// and will not include the initial packet marker, length field, and flags.
     pub fn ser(&self, buf: &mut [u8]) -> usize {
         let mut buf = bytes_mut::BytesMut::new(buf);
 
+        // serialize message
+        let mut payload_buf = [0u8; 255];
+        let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
+
+        // Currently expects a buffer with the sequence field at the start.
+        // If this is updated to include the initial packet marker, length field, and flags,
+        // uncomment.
+        //
+        // match self.protocol_version {
+        //     MavlinkVersion::V2 => {
+        //         buf.put_u8(MAV_STX_V2);
+        //         buf.put_u8(payload_len as u8);
+        //         but.put_u8(0); // incompatibility flags
+        //         buf.put_u8(0); // compatibility flags
+        //     }
+        //     MavlinkVersion::V1 => {
+        //         buf.put_u8(MAV_STX);
+        //         buf.put_u8(payload_len as u8);
+        //     }
+        // }
+
         // serialize header
         buf.put_u8(self.header.sequence);
         buf.put_u8(self.header.system_id);
@@ -164,18 +187,26 @@ impl<M: Message> MavFrame<M> {
                 buf.put_u8(self.msg.message_id() as u8); //TODO check
             }
         }
-        // serialize message
-        let mut payload_buf = [0u8; 255];
-        let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);
 
         buf.put_slice(&payload_buf[..payload_len]);
         buf.len()
     }
 
     /// Deserialize MavFrame from a slice that has been received from, for example, a socket.
+    /// The input buffer should start with the sequence field of the Mavlink frame. The
+    /// initial packet marker, length field, and flag fields should be excluded.
     pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result<Self, ParserError> {
         let mut buf = Bytes::new(input);
 
+        // Currently expects a buffer with the sequence field at the start.
+        // If this is updated to include the initial packet marker, length field, and flags,
+        // uncomment.
+        // <https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format>
+        // match version {
+        //     MavlinkVersion::V2 => buf.get_u32_le(),
+        //     MavlinkVersion::V1 => buf.get_u16_le().into(),
+        // };
+
         let sequence = buf.get_u8();
         let system_id = buf.get_u8();
         let component_id = buf.get_u8();
diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs
index d1fac61..1c225ef 100644
--- a/mavlink/tests/mav_frame_tests.rs
+++ b/mavlink/tests/mav_frame_tests.rs
@@ -5,8 +5,13 @@ mod mav_frame_tests {
     use mavlink::MavFrame;
     use mavlink::MavHeader;
 
-    // NOTE: No header
+    // NOTE: No STX, length, or flag fields in the header
     pub const HEARTBEAT_V2: &[u8] = &[
+        // Currently [`MavFrame::deser`] and [`MavFrame::ser`] does not account for the first four fields.
+        // 0xfd, // STX V2
+        // 0x09, // len
+        // 0x00, // incompat_flags
+        // 0x00, // compat_flags
         crate::test_shared::COMMON_MSG_HEADER.sequence,
         crate::test_shared::COMMON_MSG_HEADER.system_id,
         crate::test_shared::COMMON_MSG_HEADER.component_id,
-- 
GitLab