lib: Return size in write functions

src/connection/direct_serial.rs

@@ -74,7 +74,7 @@ impl<M: Message> MavConnection<M> for SerialConnection {
         }
     }
 
-    fn send(&self, header: &MavHeader, data: &M) -> Result<(), MessageWriteError> {
+    fn send(&self, header: &MavHeader, data: &M) -> Result<usize, MessageWriteError> {
         let mut port = self.port.lock().unwrap();
         let mut sequence = self.sequence.lock().unwrap();
 
@@ -86,8 +86,7 @@ impl<M: Message> MavConnection<M> 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) {

src/connection/file.rs

@@ -45,8 +45,8 @@ impl<M: Message> MavConnection<M> for FileConnection {
         }
     }
 
-    fn send(&self, _header: &MavHeader, _data: &M) -> Result<(), MessageWriteError> {
-        Ok(())
+    fn send(&self, _header: &MavHeader, _data: &M) -> Result<usize, MessageWriteError> {
+        Ok(0)
     }
 
     fn set_protocol_version(&mut self, version: MavlinkVersion) {

src/connection/mod.rs

@@ -21,13 +21,13 @@ pub trait MavConnection<M: Message> {
     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<usize, crate::error::MessageWriteError>;
 
     fn set_protocol_version(&mut self, version: MavlinkVersion);
     fn get_protocol_version(&self) -> MavlinkVersion;
 
     /// Write whole frame
-    fn send_frame(&self, frame: &MavFrame<M>) -> Result<(), crate::error::MessageWriteError> {
+    fn send_frame(&self, frame: &MavFrame<M>) -> Result<usize, crate::error::MessageWriteError> {
         self.send(&frame.header, &frame.msg)
     }
 
@@ -43,7 +43,7 @@ pub trait MavConnection<M: Message> {
     }
 
     /// Send a message with default header
-    fn send_default(&self, data: &M) -> Result<(), crate::error::MessageWriteError> {
+    fn send_default(&self, data: &M) -> Result<usize, crate::error::MessageWriteError> {
         let header = MavHeader::default();
         self.send(&header, data)
     }

src/connection/tcp.rs

@@ -92,7 +92,7 @@ impl<M: Message> MavConnection<M> 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<usize, crate::error::MessageWriteError> {
         let mut lock = self.writer.lock().unwrap();
 
         let header = MavHeader {

src/connection/udp.rs

@@ -160,7 +160,7 @@ impl<M: Message> MavConnection<M> for UdpConnection {
         }
     }
 
-    fn send(&self, header: &MavHeader, data: &M) -> Result<(), crate::error::MessageWriteError> {
+    fn send(&self, header: &MavHeader, data: &M) -> Result<usize, crate::error::MessageWriteError> {
         let mut guard = self.writer.lock().unwrap();
         let state = &mut *guard;
 
@@ -172,13 +172,15 @@ impl<M: Message> MavConnection<M> 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) {

src/lib.rs

@@ -353,7 +353,7 @@ pub fn write_versioned_msg<M: Message, W: Write>(
     version: MavlinkVersion,
     header: MavHeader,
     data: &M,
-) -> Result<(), error::MessageWriteError> {
+) -> Result<usize, error::MessageWriteError> {
     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<M: Message, W: Write>(
     w: &mut W,
     header: MavHeader,
     data: &M,
-) -> Result<(), error::MessageWriteError> {
+) -> Result<usize, error::MessageWriteError> {
     let msgid = data.message_id();
     let payload = data.ser();
     //    println!("write payload_len : {}", payload.len());
@@ -395,19 +395,23 @@ pub fn write_v2_msg<M: Message, W: Write>(
     //             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::<LittleEndian>(crc.get_crc())?;
+    w.write_all(&crc_bytes)?;
 
-    Ok(())
+    Ok(len)
 }
 
 /// Write a MAVLink v1 message to a Write stream.
 pub fn write_v1_msg<M: Message, W: Write>(
     w: &mut W,
     header: MavHeader,
     data: &M,
-) -> Result<(), error::MessageWriteError> {
+) -> Result<usize, error::MessageWriteError> {
     let msgid = data.message_id();
     let payload = data.ser();
 
@@ -425,9 +429,13 @@ pub fn write_v1_msg<M: Message, W: Write>(
     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::<LittleEndian>(crc.get_crc())?;
+    w.write_all(&crc_bytes)?;
 
-    Ok(())
+    Ok(len)
 }