From ae6dd560fdd259d1debbe72911bacf373152f7b3 Mon Sep 17 00:00:00 2001 From: Daniil Nemtsev Date: Thu, 29 Apr 2021 14:58:09 +0000 Subject: [PATCH] lib: Return size in write functions --- src/connection/direct_serial.rs | 5 ++--- src/connection/file.rs | 4 ++-- src/connection/mod.rs | 6 +++--- src/connection/tcp.rs | 2 +- src/connection/udp.rs | 12 +++++++----- src/lib.rs | 22 +++++++++++++++------- 6 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/connection/direct_serial.rs b/src/connection/direct_serial.rs index b156b38064..ab20df006d 100644 --- a/src/connection/direct_serial.rs +++ b/src/connection/direct_serial.rs @@ -74,7 +74,7 @@ impl MavConnection for SerialConnection { } } - fn send(&self, header: &MavHeader, data: &M) -> Result<(), MessageWriteError> { + fn send(&self, header: &MavHeader, data: &M) -> Result { let mut port = self.port.lock().unwrap(); let mut sequence = self.sequence.lock().unwrap(); @@ -86,8 +86,7 @@ impl MavConnection for SerialConnection { *sequence = sequence.wrapping_add(1); - write_versioned_msg(&mut *port, self.protocol_version, header, data)?; - Ok(()) + write_versioned_msg(&mut *port, self.protocol_version, header, data) } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/src/connection/file.rs b/src/connection/file.rs index e62c255ab4..777f1b280c 100644 --- a/src/connection/file.rs +++ b/src/connection/file.rs @@ -45,8 +45,8 @@ impl MavConnection for FileConnection { } } - fn send(&self, _header: &MavHeader, _data: &M) -> Result<(), MessageWriteError> { - Ok(()) + fn send(&self, _header: &MavHeader, _data: &M) -> Result { + Ok(0) } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/src/connection/mod.rs b/src/connection/mod.rs index 390d476579..10dacbe976 100644 --- a/src/connection/mod.rs +++ b/src/connection/mod.rs @@ -21,13 +21,13 @@ pub trait MavConnection { fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; /// Send a mavlink message - fn send(&self, header: &MavHeader, data: &M) -> Result<(), crate::error::MessageWriteError>; + fn send(&self, header: &MavHeader, data: &M) -> Result; fn set_protocol_version(&mut self, version: MavlinkVersion); fn get_protocol_version(&self) -> MavlinkVersion; /// Write whole frame - fn send_frame(&self, frame: &MavFrame) -> Result<(), crate::error::MessageWriteError> { + fn send_frame(&self, frame: &MavFrame) -> Result { self.send(&frame.header, &frame.msg) } @@ -43,7 +43,7 @@ pub trait MavConnection { } /// Send a message with default header - fn send_default(&self, data: &M) -> Result<(), crate::error::MessageWriteError> { + fn send_default(&self, data: &M) -> Result { let header = MavHeader::default(); self.send(&header, data) } diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs index ce6346b9aa..89a109e86e 100644 --- a/src/connection/tcp.rs +++ b/src/connection/tcp.rs @@ -92,7 +92,7 @@ impl MavConnection for TcpConnection { read_versioned_msg(&mut *lock, self.protocol_version) } - fn send(&self, header: &MavHeader, data: &M) -> Result<(), crate::error::MessageWriteError> { + fn send(&self, header: &MavHeader, data: &M) -> Result { let mut lock = self.writer.lock().unwrap(); let header = MavHeader { diff --git a/src/connection/udp.rs b/src/connection/udp.rs index ed90f204dd..e9418e9a06 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -160,7 +160,7 @@ impl MavConnection for UdpConnection { } } - fn send(&self, header: &MavHeader, data: &M) -> Result<(), crate::error::MessageWriteError> { + fn send(&self, header: &MavHeader, data: &M) -> Result { let mut guard = self.writer.lock().unwrap(); let state = &mut *guard; @@ -172,13 +172,15 @@ impl MavConnection for UdpConnection { state.sequence = state.sequence.wrapping_add(1); - if let Some(addr) = state.dest { + let len = if let Some(addr) = state.dest { let mut buf = Vec::new(); write_versioned_msg(&mut buf, self.protocol_version, header, data)?; - state.socket.send_to(&buf, addr)?; - } + state.socket.send_to(&buf, addr)? + } else { + 0 + }; - Ok(()) + Ok(len) } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/src/lib.rs b/src/lib.rs index 1abf39880d..55e6095662 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -353,7 +353,7 @@ pub fn write_versioned_msg( version: MavlinkVersion, header: MavHeader, data: &M, -) -> Result<(), error::MessageWriteError> { +) -> Result { match version { MavlinkVersion::V2 => write_v2_msg(w, header, data), MavlinkVersion::V1 => write_v1_msg(w, header, data), @@ -365,7 +365,7 @@ pub fn write_v2_msg( w: &mut W, header: MavHeader, data: &M, -) -> Result<(), error::MessageWriteError> { +) -> Result { let msgid = data.message_id(); let payload = data.ser(); // println!("write payload_len : {}", payload.len()); @@ -395,11 +395,15 @@ pub fn write_v2_msg( // 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_u16::(crc.get_crc())?; + w.write_all(&crc_bytes)?; - Ok(()) + Ok(len) } /// Write a MAVLink v1 message to a Write stream. @@ -407,7 +411,7 @@ pub fn write_v1_msg( w: &mut W, header: MavHeader, data: &M, -) -> Result<(), error::MessageWriteError> { +) -> Result { let msgid = data.message_id(); let payload = data.ser(); @@ -425,9 +429,13 @@ pub fn write_v1_msg( 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_u16::(crc.get_crc())?; + w.write_all(&crc_bytes)?; - Ok(()) + Ok(len) }