From 2904a5ff8e4656c70ef09e4be78c1296ba41b081 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Wed, 22 Mar 2023 18:03:58 +0400 Subject: [PATCH] Add MessageData trait to allow serialize ***_DATA structs into MAVLinkV[1|2]MessageRaw --- build/parser.rs | 103 +++++++-------------- src/lib.rs | 155 ++++++++++++++++++++++++-------- tests/v1_encode_decode_tests.rs | 11 +++ tests/v2_encode_decode_tests.rs | 11 +++ 4 files changed, 170 insertions(+), 110 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 94415d60a9..66dc6d30b7 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -121,28 +121,6 @@ impl MavProfile { .collect() } - /// A list of message IDs - fn emit_msg_ids(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let msg_id = msg.id; - quote!(#msg_id) - }) - .collect() - } - - /// CRC values needed for mavlink parsing - fn emit_msg_crc(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let ec = extra_crc(msg); - quote!(#ec) - }) - .collect() - } - fn emit_rust(&self) -> TokenStream { //TODO verify that id_width of u8 is OK even in mavlink v1 let id_width = format_ident!("u32"); @@ -152,17 +130,15 @@ impl MavProfile { let enum_names = self.emit_enum_names(); let struct_names = self.emit_struct_names(); let enums = self.emit_enums(); - let msg_ids = self.emit_msg_ids(); - let msg_crc = self.emit_msg_crc(); let mav_message = self.emit_mav_message(&enum_names, &struct_names); - let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names, &msg_ids); - let mav_message_crc = self.emit_mav_message_crc(&id_width, &msg_ids, &msg_crc); - let mav_message_name = self.emit_mav_message_name(&enum_names); - let mav_message_id = self.emit_mav_message_id(&enum_names, &msg_ids); - let mav_message_id_from_name = self.emit_mav_message_id_from_name(&enum_names, &msg_ids); + let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names); + let mav_message_crc = self.emit_mav_message_crc(&id_width, &struct_names); + let mav_message_name = self.emit_mav_message_name(&enum_names, &struct_names); + let mav_message_id = self.emit_mav_message_id(&enum_names, &struct_names); + let mav_message_id_from_name = self.emit_mav_message_id_from_name(&struct_names); let mav_message_default_from_id = - self.emit_mav_message_default_from_id(&enum_names, &msg_ids); + self.emit_mav_message_default_from_id(&enum_names, &struct_names); let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); quote! { @@ -181,7 +157,7 @@ impl MavProfile { #[allow(unused_imports)] use heapless::Vec; - use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut}; + use crate::{Message, MessageData, error::*, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -205,11 +181,7 @@ impl MavProfile { } } - fn emit_mav_message( - &self, - enums: &Vec, - structs: &Vec, - ) -> TokenStream { + fn emit_mav_message(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] @@ -223,14 +195,13 @@ impl MavProfile { &self, enums: &[TokenStream], structs: &[TokenStream], - ids: &[TokenStream], ) -> TokenStream { let id_width = format_ident!("u32"); quote! { fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { - #(#ids => #structs::deser(version, payload).map(Self::#enums),)* + #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* _ => { Err(ParserError::UnknownMessage { id }) }, @@ -242,13 +213,12 @@ impl MavProfile { fn emit_mav_message_crc( &self, id_width: &Ident, - ids: &[TokenStream], - crc: &[TokenStream], + structs: &[TokenStream], ) -> TokenStream { quote! { fn extra_crc(id: #id_width) -> u8 { match id { - #(#ids => #crc,)* + #(#structs::ID => #structs::EXTRA_CRC,)* _ => { 0 }, @@ -257,46 +227,32 @@ impl MavProfile { } } - fn emit_mav_message_name(&self, enums: &Vec) -> TokenStream { - let enum_names = enums.iter().map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }); - + fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { fn message_name(&self) -> &'static str { match self { - #(Self::#enums(..) => #enum_names,)* + #(Self::#enums(..) => #structs::NAME,)* } } } } - fn emit_mav_message_id(&self, enums: &Vec, ids: &Vec) -> TokenStream { + fn emit_mav_message_id(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { let id_width = format_ident!("u32"); quote! { fn message_id(&self) -> #id_width { match self { - #(Self::#enums(..) => #ids,)* + #(Self::#enums(..) => #structs::ID,)* } } } } - fn emit_mav_message_id_from_name( - &self, - enums: &[TokenStream], - ids: &[TokenStream], - ) -> TokenStream { - let enum_names = enums.iter().map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }); - + fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream { quote! { fn message_id_from_name(name: &str) -> Result { match name { - #(#enum_names => Ok(#ids),)* + #(#structs::NAME => Ok(#structs::ID),)* _ => { Err("Invalid message name.") } @@ -308,17 +264,12 @@ impl MavProfile { fn emit_mav_message_default_from_id( &self, enums: &[TokenStream], - ids: &[TokenStream], + structs: &[TokenStream], ) -> TokenStream { - let data_name = enums.iter().map(|enum_name| { - let name = format_ident!("{}_DATA", enum_name.to_string()); - quote!(#name) - }); - quote! { fn default_message_from_id(id: u32) -> Result { match id { - #(#ids => Ok(Self::#enums(#data_name::default())),)* + #(#structs::ID => Ok(Self::#enums(#structs::default())),)* _ => { Err("Invalid message id.") } @@ -584,6 +535,9 @@ impl MavMessage { fn emit_rust(&self) -> TokenStream { let msg_name = self.emit_struct_name(); + let id = self.id; + let name = self.name.clone(); + let extra_crc = extra_crc(self); let (name_types, msg_encoded_len) = self.emit_name_types(); let deser_vars = self.emit_deserialize_vars(); @@ -603,14 +557,19 @@ impl MavMessage { #(#name_types)* } - impl #msg_name { - pub const ENCODED_LEN: usize = #msg_encoded_len; + impl MessageData for #msg_name { + type Message = MavMessage; + + const ID: u32 = #id; + const NAME: &'static str = #name; + const EXTRA_CRC: u8 = #extra_crc; + const ENCODED_LEN: usize = #msg_encoded_len; - pub fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { + fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { #deser_vars } - pub fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { #serialize_vars } } diff --git a/src/lib.rs b/src/lib.rs index eb2e3616b5..188db386ed 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -83,6 +83,18 @@ where fn extra_crc(id: u32) -> u8; } +pub trait MessageData: Sized { + type Message: Message; + + const ID: u32; + const NAME: &'static str; + const EXTRA_CRC: u8; + const ENCODED_LEN: usize; + + fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize; + fn deser(version: MavlinkVersion, payload: &[u8]) -> Result; +} + /// Metadata from a MAVLink packet header #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -202,6 +214,14 @@ impl MavFrame { } } +fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(data); + + crc_calculator.digest(&[extra_crc]); + crc_calculator.get_crc() +} + pub fn read_versioned_msg( r: &mut R, version: MavlinkVersion, @@ -285,41 +305,67 @@ impl MAVLinkV1MessageRaw { &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)] } - pub fn calculate_crc(&self) -> u16 { + #[inline] + pub fn has_valid_crc(&self) -> bool { let payload_length: usize = self.payload_length().into(); - let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); - let extra_crc = M::extra_crc(self.message_id().into()); - - crc_calculator.digest(&[extra_crc]); - crc_calculator.get_crc() + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id().into()), + ) } - #[inline] - pub fn has_valid_crc(&self) -> bool { - self.checksum() == self.calculate_crc::() + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)] } - pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + ) { self.0[0] = MAV_STX; - let msgid = message.message_id(); - - let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; - let payload_len = message.ser(MavlinkVersion::V1, payload_buf); let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ - payload_len as u8, + payload_length as u8, header.sequence, header.system_id, header.component_id, msgid as u8, ]); - let crc = self.calculate_crc::(); - self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] .copy_from_slice(&crc.to_le_bytes()); } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V1, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + } } /// Return a raw buffer with the mavlink message @@ -456,42 +502,51 @@ impl MAVLinkV2MessageRaw { let payload_length: usize = self.payload_length().into(); // Signature to ensure the link is tamper-proof. - let signature_size = if (self.incompatibility_flags() & 0x01) == MAVLINK_IFLAG_SIGNED { + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { Self::SIGNATURE_SIZE } else { 0 }; &mut self.0 - [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2 + signature_size)] + [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] } - pub fn calculate_crc(&self) -> u16 { + #[inline] + pub fn has_valid_crc(&self) -> bool { let payload_length: usize = self.payload_length().into(); - let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); - - let extra_crc = M::extra_crc(self.message_id()); - crc_calculator.digest(&[extra_crc]); - crc_calculator.get_crc() + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id().into()), + ) } - #[inline] - pub fn has_valid_crc(&self) -> bool { - self.checksum() == self.calculate_crc::() + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { + Self::SIGNATURE_SIZE + } else { + 0 + }; + + &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] } - pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + ) { self.0[0] = MAV_STX_V2; - let msgid = message.message_id(); let msgid_bytes = msgid.to_le_bytes(); - let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; - let payload_len = message.ser(MavlinkVersion::V2, payload_buf); - let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ - payload_len as u8, + payload_length as u8, 0, //incompat_flags 0, //compat_flags header.sequence, @@ -502,10 +557,34 @@ impl MAVLinkV2MessageRaw { msgid_bytes[2], ]); - let crc = self.calculate_crc::(); - self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] .copy_from_slice(&crc.to_le_bytes()); } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + } } /// Return a raw buffer with the mavlink message diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index 83fab1d2e7..0f17e74426 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -89,4 +89,15 @@ mod test_v1_encode_decode { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV1MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V1); + assert!(raw_msg.has_valid_crc::()); + } } diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index 109fdc9694..db254607ab 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -156,4 +156,15 @@ mod test_v2_encode_decode { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV2MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V2); + assert!(raw_msg.has_valid_crc::()); + } }