From 68938b1b520659f656ebdd3454dafbff880daed6 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: Sat, 26 Nov 2022 16:07:55 +0400 Subject: [PATCH] MAVLinkV1MessageRaw and MAVLinkV2MessageRaw refactoring --- build/parser.rs | 16 +- src/bytes_mut.rs | 30 +--- src/embedded.rs | 14 -- src/lib.rs | 405 ++++++++++++++++++++++++++++------------------- src/utils.rs | 8 +- 5 files changed, 259 insertions(+), 214 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 7cb927ea2f..d9e6d2b9ec 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -182,7 +182,7 @@ impl MavProfile { #[allow(unused_imports)] use heapless::Vec; - use crate::{Message, error::*, MAX_FRAME_SIZE, bytes::Bytes, bytes_mut::BytesMut}; + use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -330,9 +330,9 @@ impl MavProfile { fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { - fn ser(&self, version: MavlinkVersion) -> BytesMut { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { match *self { - #(MavMessage::#enums(ref body) => body.ser(version),)* + #(MavMessage::#enums(ref body) => body.ser(version, bytes),)* } } } @@ -542,12 +542,14 @@ impl MavMessage { fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self.fields.iter().map(|f| f.rust_writer()); quote! { - let mut _tmp = BytesMut::new(); + let mut _tmp = BytesMut::new(bytes); #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { - crate::remove_trailing_zeroes(&mut _tmp); + let len = _tmp.len(); + crate::remove_trailing_zeroes(&mut bytes[..len]) + } else { + _tmp.len() } - _tmp } } @@ -612,7 +614,7 @@ impl MavMessage { #deser_vars } - pub fn ser(&self, version: MavlinkVersion) -> BytesMut { + pub fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { #serialize_vars } } diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index 3d3370046f..a3b70ad45c 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -1,14 +1,11 @@ -pub struct BytesMut { - data: [u8; N], +pub struct BytesMut<'a> { + data: &'a mut [u8], len: usize, } -impl BytesMut { - pub fn new() -> Self { - Self { - data: [0; N], - len: 0, - } +impl<'a> BytesMut<'a> { + pub fn new(data: &'a mut [u8]) -> Self { + Self { data, len: 0 } } #[inline] @@ -18,13 +15,7 @@ impl BytesMut { #[inline] pub fn remaining(&self) -> usize { - N - self.len - } - - pub fn set_len(&mut self, len: usize) { - assert!(len >= 1); - assert!(len <= N); - self.len = len; + self.data.len() - self.len } #[inline] @@ -132,12 +123,3 @@ impl BytesMut { self.len += SIZE; } } - -impl core::ops::Deref for BytesMut { - type Target = [u8]; - - #[inline] - fn deref(&self) -> &[u8] { - &self.data[..self.len] - } -} diff --git a/src/embedded.rs b/src/embedded.rs index 3c27b8f1bb..64138a5b14 100644 --- a/src/embedded.rs +++ b/src/embedded.rs @@ -1,17 +1,9 @@ -use byteorder::ByteOrder; - use crate::error::*; /// Replacement for std::io::Read + byteorder::ReadBytesExt in no_std envs pub trait Read { fn read_u8(&mut self) -> Result; - fn read_u16(&mut self) -> Result { - let mut buffer = [0; 2]; - self.read_exact(&mut buffer)?; - Ok(B::read_u16(&buffer)) - } - fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { for i in 0..buf.len() { buf[i] = self.read_u8()?; @@ -30,12 +22,6 @@ impl> Read for R { /// Replacement for std::io::Write + byteorder::WriteBytesExt in no_std envs pub trait Write { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError>; - - fn write_u16(&mut self, n: u16) -> Result<(), MessageWriteError> { - let mut buffer = [0; 2]; - B::write_u16(&mut buffer, n); - self.write_all(&buffer) - } } impl> Write for W { diff --git a/src/lib.rs b/src/lib.rs index 3699d0d859..40c14f200e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -25,7 +25,6 @@ use core::result::Result; #[cfg(feature = "std")] use std::io::{Read, Write}; -use byteorder::LittleEndian; #[cfg(feature = "std")] use byteorder::ReadBytesExt; @@ -43,7 +42,7 @@ use utils::remove_trailing_zeroes; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -use crate::{bytes::Bytes, bytes_mut::BytesMut, error::ParserError}; +use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; @@ -67,7 +66,9 @@ where { fn message_id(&self) -> u32; fn message_name(&self) -> &'static str; - fn ser(&self, version: MavlinkVersion) -> BytesMut; + + /// Serialize **Message** into byte slice and return count of bytes written + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize; fn parse( version: MavlinkVersion, @@ -157,8 +158,10 @@ impl MavFrame { } } // serialize message - v.extend_from_slice(&mut self.msg.ser(self.protocol_version)) - .unwrap(); + let mut payload_buf = [0u8; 255]; + let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); + + v.extend_from_slice(&payload_buf[..payload_len]).unwrap(); v } @@ -178,11 +181,11 @@ impl MavFrame { let msg_id = match version { MavlinkVersion::V2 => buf.get_u32_le(), - MavlinkVersion::V1 => buf.get_u8() as u32, + MavlinkVersion::V1 => buf.get_u8().into(), }; match M::parse(version, msg_id, buf.remaining_bytes()) { - Ok(msg) => Ok(MavFrame { + Ok(msg) => Ok(Self { header, msg, protocol_version: version, @@ -209,45 +212,111 @@ pub fn read_versioned_msg( #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: https://mavlink.io/en/guide/serialization.html#v1_packet_format -pub struct MAVLinkV1MessageRaw { - pub start: u8, // = MAV_STX - pub payload_length: u8, - pub sequence: u8, - pub system_id: u8, - pub component_id: u8, - pub message_id: u8, - pub payload_buffer: [u8; 255], - pub checksum: u16, +pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); + +impl Default for MAVLinkV1MessageRaw { + fn default() -> Self { + Self::new() + } } impl MAVLinkV1MessageRaw { + const HEADER_SIZE: usize = 5; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2]) + } + + #[inline] + pub fn header(&mut self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn message_id(&self) -> u8 { + self.0[5] + } + + #[inline] pub fn payload(&self) -> &[u8] { - &self.payload_buffer[..self.payload_length as usize] + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] + } + + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) } - pub fn mut_payload(&mut self) -> &mut [u8] { - &mut self.payload_buffer[..self.payload_length as usize] + #[inline] + fn mut_payload_and_checksum(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)] } pub fn calculate_crc(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&[ - self.payload_length, - self.sequence, - self.system_id, - self.component_id, - self.message_id, - ]); - let payload = self.payload(); - crc_calculator.digest(payload); - let extra_crc = M::extra_crc(self.message_id.into()); + 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() } + #[inline] pub fn has_valid_crc(&self) -> bool { - self.checksum == self.calculate_crc::() + self.checksum() == self.calculate_crc::() + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + 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, + 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)] + .copy_from_slice(&crc.to_le_bytes()); } } @@ -263,21 +332,11 @@ pub fn read_v1_raw_message( } } - let mut message = MAVLinkV1MessageRaw { - start: MAV_STX, - payload_length: reader.read_u8()?, - sequence: reader.read_u8()?, - system_id: reader.read_u8()?, - component_id: reader.read_u8()?, - message_id: reader.read_u8()?, - payload_buffer: [0; 255], - checksum: 0, - }; - - let payload = &mut message.payload_buffer[..message.payload_length as usize]; - reader.read_exact(payload)?; + let mut message = MAVLinkV1MessageRaw::new(); - message.checksum = reader.read_u16::()?; + message.0[0] = MAV_STX; + reader.read_exact(message.mut_header())?; + reader.read_exact(message.mut_payload_and_checksum())?; Ok(message) } @@ -294,15 +353,15 @@ pub fn read_v1_msg( return M::parse( MavlinkVersion::V1, - message.message_id as u32, + u32::from(message.message_id()), message.payload(), ) .map(|msg| { ( MavHeader { - sequence: message.sequence, - system_id: message.system_id, - component_id: message.component_id, + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), }, msg, ) @@ -315,49 +374,135 @@ const MAVLINK_IFLAG_SIGNED: u8 = 0x01; #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format -pub struct MAVLinkV2MessageRaw { - pub start: u8, // = MAV_STX_V2 - pub payload_length: u8, - pub incompatibility_flags: u8, - pub compatibility_flags: u8, - pub sequence: u8, - pub system_id: u8, - pub component_id: u8, - pub message_id: u32, - pub payload_buffer: [u8; 255], - pub checksum: u16, +pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]); + +impl Default for MAVLinkV2MessageRaw { + fn default() -> Self { + Self::new() + } } impl MAVLinkV2MessageRaw { + const HEADER_SIZE: usize = 9; + const SIGNATURE_SIZE: usize = 13; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]) + } + + #[inline] + pub fn header(&mut self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn incompatibility_flags(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn compatibility_flags(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[5] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[6] + } + + #[inline] + pub fn message_id(&self) -> u32 { + u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0]) + } + + #[inline] pub fn payload(&self) -> &[u8] { - &self.payload_buffer[..self.payload_length as usize] + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] } - pub fn mut_payload(&mut self) -> &mut [u8] { - &mut self.payload_buffer[..self.payload_length as usize] + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) + } + + fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] { + 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 { + Self::SIGNATURE_SIZE + } else { + 0 + }; + + &mut self.0 + [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2 + signature_size)] } pub fn calculate_crc(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&[ - self.payload_length, - self.incompatibility_flags, - self.compatibility_flags, - self.sequence, - self.system_id, - self.component_id, - ]); - let payload = self.payload(); - crc_calculator.digest(&self.message_id.to_le_bytes()[..3]); - crc_calculator.digest(payload); - let extra_crc = M::extra_crc(self.message_id); + 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() } + #[inline] pub fn has_valid_crc(&self) -> bool { - self.checksum == self.calculate_crc::() + self.checksum() == self.calculate_crc::() + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + 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, + 0, //incompat_flags + 0, //compat_flags + header.sequence, + header.system_id, + header.component_id, + msgid_bytes[0], + msgid_bytes[1], + msgid_bytes[2], + ]); + + let crc = self.calculate_crc::(); + self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + .copy_from_slice(&crc.to_le_bytes()); } } @@ -373,31 +518,11 @@ pub fn read_v2_raw_message( } } - let mut message = MAVLinkV2MessageRaw { - start: MAV_STX_V2, - payload_length: reader.read_u8()?, - incompatibility_flags: reader.read_u8()?, - compatibility_flags: reader.read_u8()?, - sequence: reader.read_u8()?, - system_id: reader.read_u8()?, - component_id: reader.read_u8()?, - message_id: 0, - payload_buffer: [0; 255], - checksum: 0, - }; - - message.message_id = - u32::from_le_bytes([reader.read_u8()?, reader.read_u8()?, reader.read_u8()?, 0]); - - reader.read_exact(message.mut_payload())?; - - message.checksum = reader.read_u16::()?; + let mut message = MAVLinkV2MessageRaw::new(); - // Signature to ensure the link is tamper-proof. - if (message.incompatibility_flags & 0x01) == MAVLINK_IFLAG_SIGNED { - let mut sign = [0; 13]; - reader.read_exact(&mut sign)?; - } + message.0[0] = MAV_STX_V2; + reader.read_exact(message.mut_header())?; + reader.read_exact(message.mut_payload_and_checksum_and_sign())?; Ok(message) } @@ -413,13 +538,13 @@ pub fn read_v2_msg( continue; } - return M::parse(MavlinkVersion::V2, message.message_id, message.payload()) + return M::parse(MavlinkVersion::V2, message.message_id(), message.payload()) .map(|msg| { ( MavHeader { - sequence: message.sequence, - system_id: message.system_id, - component_id: message.component_id, + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), }, msg, ) @@ -447,42 +572,13 @@ pub fn write_v2_msg( header: MavHeader, data: &M, ) -> Result { - let msgid = data.message_id(); - let payload = data.ser(MavlinkVersion::V2); - // println!("write payload_len : {}", payload.len()); - - let header = &[ - MAV_STX_V2, - payload.len() as u8, - 0, //incompat_flags - 0, //compat_flags - header.sequence, - header.system_id, - header.component_id, - (msgid & 0x0000FF) as u8, - ((msgid & 0x00FF00) >> 8) as u8, - ((msgid & 0xFF0000) >> 16) as u8, - ]; - - // println!("write H: {:?}",header ); - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - // let header_crc = crc.get_crc(); - crc.digest(&payload[..]); - // let base_crc = crc.get_crc(); - let extra_crc = M::extra_crc(msgid); - // println!("write header_crc: {} base_crc: {} extra_crc: {}", - // header_crc, base_crc, extra_crc); - crc.digest(&[extra_crc]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; + let mut message_raw = MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len])?; Ok(len) } @@ -493,30 +589,13 @@ pub fn write_v1_msg( header: MavHeader, data: &M, ) -> Result { - let msgid = data.message_id(); - let payload = data.ser(MavlinkVersion::V1); - - let header = &[ - MAV_STX, - payload.len() as u8, - header.sequence, - header.system_id, - header.component_id, - msgid as u8, - ]; - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - crc.digest(&payload[..]); - crc.digest(&[M::extra_crc(msgid)]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; + let mut message_raw = MAVLinkV1MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len])?; Ok(len) } diff --git a/src/utils.rs b/src/utils.rs index d89058526a..2f43bbbbac 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,14 +1,10 @@ -use crate::{bytes_mut::BytesMut, MAX_FRAME_SIZE}; - /// Removes the trailing zeroes in the payload /// /// # Note: /// /// There must always be at least one remaining byte even if it is a /// zero byte. -#[allow(dead_code)] -pub(crate) fn remove_trailing_zeroes(buf: &mut BytesMut) { - let data = &**buf; +pub(crate) fn remove_trailing_zeroes<'a>(data: &mut [u8]) -> usize { let mut len = data.len(); for b in data[1..].iter().rev() { @@ -19,5 +15,5 @@ pub(crate) fn remove_trailing_zeroes(buf: &mut BytesMut) { len -= 1; } - buf.set_len(len); + len }