Skip to content

Commit

Permalink
fix: add MavFrame deser & ser warning, fixes #250
Browse files Browse the repository at this point in the history
  • Loading branch information
amsmith-pro authored and patrickelectric committed Aug 22, 2024
1 parent 311cd65 commit 4faca0e
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 5 deletions.
39 changes: 35 additions & 4 deletions mavlink-core/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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> {
Expand All @@ -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);
Expand All @@ -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();
Expand Down
7 changes: 6 additions & 1 deletion mavlink/tests/mav_frame_tests.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down

0 comments on commit 4faca0e

Please sign in to comment.