diff --git a/src/lib.rs b/src/lib.rs index 55e6095662..6aabb3d6bf 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -145,15 +145,15 @@ impl MavFrame { let mut v = vec![]; // serialize header + v.push(self.header.sequence); v.push(self.header.system_id); v.push(self.header.component_id); - v.push(self.header.sequence); // message id match self.protocol_version { MavlinkVersion::V2 => { let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); - v.extend_from_slice(&bytes); + v.extend_from_slice(&bytes[..3]); } MavlinkVersion::V1 => { v.push(self.msg.message_id() as u8); //TODO check @@ -170,9 +170,9 @@ impl MavFrame { pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { let mut buf = BytesMut::from(input); + let sequence = buf.get_u8(); let system_id = buf.get_u8(); let component_id = buf.get_u8(); - let sequence = buf.get_u8(); let header = MavHeader { system_id, component_id, @@ -180,7 +180,7 @@ impl MavFrame { }; let msg_id = match version { - MavlinkVersion::V2 => buf.get_u32_le(), + MavlinkVersion::V2 => buf.get_int_le(3) as u32, MavlinkVersion::V1 => buf.get_u8() as u32, }; diff --git a/tests/mav_frame_tests.rs b/tests/mav_frame_tests.rs new file mode 100644 index 0000000000..33399fb376 --- /dev/null +++ b/tests/mav_frame_tests.rs @@ -0,0 +1,96 @@ +pub mod test_shared; + +mod mav_frame_tests { + use mavlink::common::MavMessage; + use mavlink::MavFrame; + use mavlink::MavHeader; + + // NOTE: No header, based over get_heartbeat_msg + pub const HEARTBEAT_V2: &[u8] = &[ + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload - custom_mode + 0x00, // + 0x00, // + 0x00, // + 0x02, // mav_type + 0x03, // autopilot + 0x59, // base_mode + 0x03, // system_status + 0x03, // mavlink_version + 0x10, // checksum + 0xf0, + ]; + + #[test] + pub fn test_deser_ser() { + use mavlink::{common::MavMessage, MavFrame, MavlinkVersion}; + let frame = MavFrame::::deser(MavlinkVersion::V2, HEARTBEAT_V2) + .expect("failed to parse message"); + + assert_eq!(frame.header, crate::test_shared::COMMON_MSG_HEADER); + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + + let buffer = frame.ser(); + assert_eq!(buffer, HEARTBEAT_V2[..buffer.len()]); + + let MavMessage::HEARTBEAT(msg) = frame.msg else { + panic!("Decoded wrong message type"); + }; + assert_eq!(msg.custom_mode, heartbeat_msg.custom_mode); + assert_eq!(msg.mavtype, heartbeat_msg.mavtype); + assert_eq!(msg.autopilot, heartbeat_msg.autopilot); + assert_eq!(msg.base_mode, heartbeat_msg.base_mode); + assert_eq!(msg.system_status, heartbeat_msg.system_status); + assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); + } + + #[test] + pub fn test_deser_ser_message() { + let mavlink_message = mavlink_message(); + let mavlink_frame = new(mavlink_message); + + let buffer = mavlink_frame.ser(); + + let parsed_mavlink_frame = + MavFrame::::deser(mavlink::MavlinkVersion::V2, &buffer) + .unwrap(); + + assert_eq!( + format!("{mavlink_frame:?}"), + format!("{parsed_mavlink_frame:?}") + ); + } + + fn mavlink_message() -> mavlink::common::MavMessage { + mavlink::common::MavMessage::LINK_NODE_STATUS(mavlink::common::LINK_NODE_STATUS_DATA { + timestamp: 92197916, + tx_rate: 0x11223344, + rx_rate: 0x55667788, + messages_sent: 0x99001122, + messages_received: 0x33445566, + messages_lost: 0x77889900, + rx_parse_err: 0x1122, + tx_overflows: 0x3355, + rx_overflows: 0x5566, + tx_buf: 0xff, + rx_buf: 0x11, + }) + } + + fn new(msg: MavMessage) -> MavFrame { + MavFrame { + header: MavHeader { + system_id: 1, + component_id: 2, + sequence: 84, + }, + msg, + protocol_version: mavlink::MavlinkVersion::V2, + } + } +} diff --git a/tests/test_shared/mod.rs b/tests/test_shared/mod.rs index 35ba39a52e..c66945540d 100644 --- a/tests/test_shared/mod.rs +++ b/tests/test_shared/mod.rs @@ -3,7 +3,7 @@ extern crate mavlink; pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { sequence: 239, system_id: 1, - component_id: 1, + component_id: 2, }; #[cfg(feature = "common")] diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index ad3addc167..3bdcddf6be 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -9,9 +9,9 @@ mod test_v1_encode_decode { pub const HEARTBEAT_V1: &'static [u8] = &[ mavlink::MAV_STX, 0x09, - 0xef, - 0x01, - 0x01, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x05, 0x00, @@ -22,8 +22,8 @@ mod test_v1_encode_decode { 0x59, 0x03, 0x03, - 0xf1, - 0xd7, + 0x1f, + 0x50, ]; #[test] diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index b6cfd8b2fa..ef28fa577a 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -10,9 +10,9 @@ mod test_v2_encode_decode { 0x09, //payload len 0, //incompat flags 0, //compat flags - 0xef, //seq 239 - 0x01, //sys ID - 0x01, //comp ID + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x00, 0x00, //msg ID @@ -25,8 +25,8 @@ mod test_v2_encode_decode { 0x59, 0x03, 0x03, //payload - 16, - 240, //checksum + 46, + 115, //checksum ]; #[test]