WIP: Try recv

Cargo.toml

@@ -54,7 +54,7 @@ byteorder = { version = "1.3.4", optional = true }
 "udp" = []
 "tcp" = []
 "direct-serial" = []
-default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "common"]
+default= ["std", "udp", "serde", "common"]
 
 # build with all features on docs.rs so that users viewing documentation
 # can see everything

src/bin/mavlink-dump.rs

@@ -27,7 +27,7 @@ fn main() {
 
     // here as an example we force the protocol version to mavlink V1:
     // the default for this library is mavlink V2
-    mavconn.set_protocol_version(mavlink::MavlinkVersion::V1);
+    mavconn.set_protocol_version(mavlink::MavlinkVersion::V2);
 
     let vehicle = Arc::new(mavconn);
     vehicle
@@ -63,7 +63,7 @@ fn main() {
                     }
                     _ => {
                         println!("recv error: {:?}", e);
-                        break;
+                        //break;
                     }
                 }
             }

src/connection/file.rs

@@ -1,6 +1,6 @@
 use crate::connection::MavConnection;
 use crate::error::MessageReadError;
-use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message};
+use crate::{MavHeader, MavlinkVersion, Message};
 use std::fs::File;
 use std::io::{self};
 use std::sync::Mutex;
@@ -15,13 +15,13 @@ pub fn open(file_path: &str) -> io::Result<FileConnection> {
 
     Ok(FileConnection {
         file: Mutex::new(file),
-        protocol_version: MavlinkVersion::V2,
+        //protocol_version: MavlinkVersion::V2,
     })
 }
 
 pub struct FileConnection {
     file: Mutex<std::fs::File>,
-    protocol_version: MavlinkVersion,
+    //protocol_version: MavlinkVersion,
 }
 
 impl<M: Message> MavConnection<M> for FileConnection {
@@ -31,6 +31,7 @@ impl<M: Message> MavConnection<M> for FileConnection {
         let mut file = self.file.lock().unwrap();
 
         loop {
+            /*
             match read_versioned_msg(&mut *file, self.protocol_version) {
                 ok @ Ok(..) => {
                     return ok;
@@ -42,6 +43,7 @@ impl<M: Message> MavConnection<M> for FileConnection {
                 }
                 _ => {}
             }
+            */
         }
     }
 
@@ -50,10 +52,11 @@ impl<M: Message> MavConnection<M> for FileConnection {
     }
 
     fn set_protocol_version(&mut self, version: MavlinkVersion) {
-        self.protocol_version = version;
+        //self.protocol_version = version;
     }
 
     fn get_protocol_version(&self) -> MavlinkVersion {
-        self.protocol_version
+        //self.protocol_version
+        MavlinkVersion::V1
     }
 }

src/connection/udp.rs

@@ -1,5 +1,5 @@
 use crate::connection::MavConnection;
-use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message};
+use crate::{MavlinkParser, MavlinkV1Parser, write_versioned_msg, MavHeader, MavlinkVersion, Message};
 use std::io::Read;
 use std::io::{self};
 use std::net::ToSocketAddrs;
@@ -13,11 +13,11 @@ pub fn select_protocol<M: Message>(
     address: &str,
 ) -> io::Result<Box<dyn MavConnection<M> + Sync + Send>> {
     if address.starts_with("udpin:") {
-        Ok(Box::new(udpin(&address["udpin:".len()..])?))
+        Ok(Box::new(udpin<M>(&address["udpin:".len()..])?))
     } else if address.starts_with("udpout:") {
-        Ok(Box::new(udpout(&address["udpout:".len()..])?))
+        Ok(Box::new(udpout<M>(&address["udpout:".len()..])?))
     } else if address.starts_with("udpbcast:") {
-        Ok(Box::new(udpbcast(&address["udpbcast:".len()..])?))
+        Ok(Box::new(udpbcast<M>(&address["udpbcast:".len()..])?))
     } else {
         Err(io::Error::new(
             io::ErrorKind::AddrNotAvailable,
@@ -26,7 +26,7 @@ pub fn select_protocol<M: Message>(
     }
 }
 
-pub fn udpbcast<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
+pub fn udpbcast<M: Message, T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
     let addr = address
         .to_socket_addrs()
         .unwrap()
@@ -36,7 +36,7 @@ pub fn udpbcast<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
     socket
         .set_broadcast(true)
         .expect("Couldn't bind to broadcast address.");
-    UdpConnection::new(socket, false, Some(addr))
+    UdpConnection<M>::new(socket, false, Some(addr))
 }
 
 pub fn udpout<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
@@ -114,11 +114,12 @@ struct UdpRead {
     recv_buf: PacketBuf,
 }
 
-pub struct UdpConnection {
+pub struct UdpConnection<M: Message> {
     reader: Mutex<UdpRead>,
     writer: Mutex<UdpWrite>,
-    protocol_version: MavlinkVersion,
     server: bool,
+    protocol_version: MavlinkVersion,
+    parser: Box<dyn MavlinkParser>,
 }
 
 impl UdpConnection {
@@ -134,7 +135,8 @@ impl UdpConnection {
                 dest: dest,
                 sequence: 0,
             }),
-            protocol_version: MavlinkVersion::V2,
+            protocol_version: MavlinkVersion::V1,
+            parser: MavlinkV1Parser::default(),
         })
     }
 }
@@ -152,8 +154,16 @@ impl<M: Message> MavConnection<M> for UdpConnection {
                     self.writer.lock().unwrap().dest = Some(src);
                 }
             }
+            println!("Received buffer: {} start {} end {}", state.recv_buf.len(), state.recv_buf.start, state.recv_buf.end);
 
-            match read_versioned_msg(&mut state.recv_buf, self.protocol_version) {
+            /*
+            return Err(crate::error::MessageReadError::Io(std::io::Error::new(
+                std::io::ErrorKind::TimedOut,
+                "No data avaiable.",
+            )));
+            */
+
+            match self.parser.read(&mut state.recv_buf, self.protocol_version) {
                 ok @ Ok(..) => return ok,
                 _ => (),
             }
@@ -174,7 +184,7 @@ impl<M: Message> MavConnection<M> for UdpConnection {
 
         if let Some(addr) = state.dest {
             let mut buf = Vec::new();
-            write_versioned_msg(&mut buf, self.protocol_version, header, data)?;
+            //write_versioned_msg(&mut buf, self.protocol_version, header, data)?;
             state.socket.send_to(&buf, addr)?;
         }
 
@@ -188,4 +198,4 @@ impl<M: Message> MavConnection<M> for UdpConnection {
     fn get_protocol_version(&self) -> MavlinkVersion {
         self.protocol_version
     }
-}
+}

src/error.rs

@@ -5,6 +5,7 @@ use std::fmt::{Display, Formatter};
 pub enum ParserError {
     InvalidFlag { flag_type: String, value: u32 },
     InvalidEnum { enum_type: String, value: u32 },
+    WrongCrc { expected: u16, received: u16 },
     UnknownMessage { id: u32 },
 }
 
@@ -21,6 +22,11 @@ impl Display for ParserError {
                 "Invalid enum value for enum type {:?}, got {:?}",
                 enum_type, value
             ),
+            ParserError::WrongCrc { expected, received } => write!(
+                f,
+                "CRC does not mache, expected {:?}, got {:?}",
+                expected, received
+            ),
             ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {:?}", id),
         }
     }