diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 342d7297b9..5583793a42 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 { @@ -146,9 +146,32 @@ impl MavFrame { // } /// 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 MavFrame { 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 { 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. + // + // 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 d1fac6193f..1c225efd09 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,