diff --git a/Cargo.toml b/Cargo.toml
index 48ae644425..00aa2d16a9 100644
--- a/Cargo.toml
+++ b/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
diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs
index efa2967c8f..a0f3cee2a1 100644
--- a/src/bin/mavlink-dump.rs
+++ b/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;
                     }
                 }
             }
diff --git a/src/connection/file.rs b/src/connection/file.rs
index 5cc64b58c5..da9fd445cc 100644
--- a/src/connection/file.rs
+++ b/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
     }
 }
diff --git a/src/connection/udp.rs b/src/connection/udp.rs
index 87f66d6fa2..376592eb36 100644
--- a/src/connection/udp.rs
+++ b/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
     }
-}
+}
\ No newline at end of file
diff --git a/src/error.rs b/src/error.rs
index 63422a22c9..53288ab5a5 100644
--- a/src/error.rs
+++ b/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),
         }
     }
diff --git a/src/lib.rs b/src/lib.rs
index 15cebb3673..208cf3d60c 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -191,6 +191,300 @@ impl<M: Message> MavFrame<M> {
     }
 }
 
+trait MavlinkPacketFormat {
+    fn checksum(&self) -> u16;
+
+    fn set_checksum(&mut self, value: u16);
+
+    fn calc_checksum<M: Message>(&self) -> u16;
+
+    fn update_checksum<M: Message>(&mut self) {
+        self.set_checksum(self.calc_checksum::<M>());
+    }
+
+    fn validate_checksum<M: Message>(&self) -> bool {
+        return self.checksum() == self.calc_checksum::<M>();
+    }
+}
+
+/// MAVLink V1 packet format: https://mavlink.io/en/guide/serialization.html#v1_packet_format
+struct MavlinkV1PacketFormat {
+    pub magic: u8,
+    pub len: u8,
+    pub seq: u8,
+    pub sysid: u8,
+    pub compid: u8,
+    pub msgid: u8,
+    pub payload: Vec<u8>,
+    pub checksum: u16,
+}
+
+impl Default for MavlinkV1PacketFormat {
+    fn default() -> Self {
+        MavlinkV1PacketFormat {
+            magic: MAV_STX,
+            ..Default::default()
+        }
+    }
+}
+
+impl MavlinkPacketFormat for MavlinkV1PacketFormat {
+    fn checksum(&self) -> u16 {
+        return self.checksum;
+    }
+
+    fn set_checksum(&mut self, value: u16){
+        self.checksum = value;
+    }
+
+    fn calc_checksum<M: Message>(&self) -> u16 {
+        let mut crc_calculator = CRCu16::crc16mcrf4cc();
+        crc_calculator.digest(&[self.len, self.seq, self.sysid, self.compid, self.msgid]);
+        crc_calculator.digest(&self.payload);
+        crc_calculator.digest(&[M::extra_crc(self.msgid.into())]);
+
+        return crc_calculator.get_crc();
+    }
+}
+
+/// MAVLink V2 packet format: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format
+struct MavlinkV2PacketFormat {
+    pub magic: u8,
+    pub len: u8,
+    pub incompat_flags: u8,
+    pub compat_flags: u8,
+    pub seq: u8,
+    pub sysid: u8,
+    pub compid: u8,
+    pub msgid: [u8; 3],
+    pub payload: Vec<u8>,
+    pub checksum: u16,
+    pub signature: [u8; 13],
+}
+
+impl Default for MavlinkV2PacketFormat {
+    fn default() -> Self {
+        MavlinkV2PacketFormat {
+            magic: MAV_STX_V2,
+            ..Default::default()
+        }
+    }
+}
+
+impl MavlinkPacketFormat for MavlinkV2PacketFormat {
+    fn checksum(&self) -> u16 {
+        return self.checksum;
+    }
+
+    fn set_checksum(&mut self, value: u16){
+        self.checksum = value;
+    }
+
+    fn calc_checksum<M: Message>(&self) -> u16 {
+        let mut crc_calculator = CRCu16::crc16mcrf4cc();
+        crc_calculator.digest(&[
+            self.len,
+            self.incompat_flags,
+            self.compat_flags,
+            self.seq,
+            self.sysid,
+            self.compid,
+        ]);
+        crc_calculator.digest(&self.msgid);
+        crc_calculator.digest(&self.payload);
+
+        let mut msgid_buffer = [0 as u8; 4];
+        msgid_buffer[..3].copy_from_slice(&self.msgid);
+        let msgid = u32::from_le_bytes(msgid_buffer);
+
+        crc_calculator.digest(&[M::extra_crc(msgid)]);
+        return crc_calculator.get_crc();
+    }
+}
+
+trait MavlinkParser {
+    fn version(&self) -> MavlinkVersion;
+
+    fn read<M: Message>(
+        &mut self,
+        buffer: &Vec<u8>
+    ) -> Result<(MavHeader, M), error::MessageReadError>;
+}
+
+enum MavlinkV1ParserState {
+    Magic,
+    Len,
+    Seq,
+    SysId,
+    CompId,
+    MsgId,
+    Payload,
+    Crc1,
+    Crc2,
+    Done,
+}
+
+struct MavlinkV1Parser {
+    format: MavlinkV1PacketFormat,
+    state: MavlinkV1ParserState,
+    payload_parsed: u8,
+}
+
+impl MavlinkV1Parser {
+    pub fn new() -> Self {
+        MavlinkV1Parser {
+            format: Default::default(),
+            state: MavlinkV1ParserState::Len,
+            payload_parsed: 0,
+        }
+    }
+}
+
+enum MavlinkV2ParserState {
+    Magic,
+    Len,
+    IncompatFlags,
+    CompatFlags,
+    Seq,
+    SysId,
+    CompId,
+    MsgId1,
+    MsgId2,
+    MsgId3,
+    Payload,
+    Crc1,
+    Crc2,
+    Signature,
+    Done,
+}
+
+struct MavlinkV2Parser {
+    format: MavlinkV2PacketFormat,
+    state: MavlinkV2ParserState,
+    payload_parsed: u8,
+    signature_parsed: u8,
+}
+
+impl Default for MavlinkV2Parser {
+    fn default() -> Self {
+        MavlinkV2Parser {
+            format: Default::default(),
+            state: MavlinkV2ParserState::Len,
+            payload_parsed: 0,
+            signature_parsed: 0,
+        }
+    }
+}
+
+impl MavlinkParser for MavlinkV1Parser {
+    fn version(&self) -> MavlinkVersion {
+        MavlinkVersion::V1
+    }
+
+    fn read<M: Message>(
+        &mut self,
+        buffer: &Vec<u8>,
+    ) -> Result<(MavHeader, M), error::MessageReadError> {
+        let mut counter = 0;
+        //TODO: move to iterator
+        let mut get = || -> Option<u8> {
+            let value = buffer.get(counter);
+            counter += 1;
+            return value;
+        };
+
+        loop {
+            /*
+            let mut buffer = [0; 1];
+            let size = match reader.read(&mut buffer) {
+                Ok(size) => size,
+                _ => {
+                    return Err(error::MessageReadError::Io(std::io::Error::new(
+                        std::io::ErrorKind::TimedOut,
+                        "No data avaiable.",
+                    )));
+                }
+            };*/
+            use MavlinkV1ParserState::*;
+            match self.state {
+                Magic => {
+                    if buffer[0] == self.format.magic {
+                        self.state = Len;
+                    }
+                }
+                Len => {
+                    self.format.len = buffer[0];
+                    self.format.payload = vec![0; self.format.len as usize];
+                    self.state = Seq;
+                }
+                Seq => {
+                    self.format.seq = buffer[0];
+                    self.state = SysId;
+                }
+                SysId => {
+                    self.format.sysid = buffer[0];
+                    self.state = CompId;
+                }
+                CompId => {
+                    self.format.compid = buffer[0];
+                    self.state = MsgId;
+                }
+                MsgId => {
+                    self.format.msgid = buffer[0];
+                    self.state = Payload;
+                }
+                Payload => {
+                    self.format.payload[self.payload_parsed as usize] = buffer[0];
+                    self.payload_parsed += 1;
+                    if self.payload_parsed == self.format.len {
+                        self.state = Crc1;
+                    }
+                }
+                Crc1 => {
+                    let mut value = self.format.checksum.to_le_bytes();
+                    value[0] = buffer[0];
+                    self.format.checksum = u16::from_le_bytes(value);
+                    self.state = Crc2;
+                }
+                Crc2 => {
+                    let mut value = self.format.checksum.to_le_bytes();
+                    value[1] = buffer[0];
+                    self.format.checksum = u16::from_le_bytes(value);
+                    self.state = Done;
+
+                    if self.format.validate_checksum::<M>() {
+                        return M::parse(
+                            self.version(),
+                            self.format.msgid as u32,
+                            &self.format.payload,
+                        )
+                        .map(|msg| {
+                            (
+                                MavHeader {
+                                    sequence: self.format.seq,
+                                    system_id: self.format.sysid,
+                                    component_id: self.format.compid,
+                                },
+                                msg,
+                            )
+                        })
+                        .map_err(|err| err.into());
+                    }
+
+                    return Err(error::MessageReadError::Parse(ParserError::WrongCrc {
+                        expected: self.format.calc_checksum::<M>(),
+                        received: self.format.checksum,
+                    }));
+                }
+                Done => {
+                    self.state = Magic;
+                }
+            }
+        }
+    }
+}
+
+/*
 pub fn read_versioned_msg<M: Message, R: Read>(
     r: &mut R,
     version: MavlinkVersion,
@@ -337,6 +631,7 @@ pub fn read_v2_msg<M: Message, R: Read>(
             .map_err(|err| err.into());
     }
 }
+*/
 
 /// Write a message using the given mavlink version
 pub fn write_versioned_msg<M: Message, W: Write>(