diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs
index 85c771b22d868602f3fb5a78f3619dec4c580a7e..f96135956038767e1b2d9405a92be475178cc126 100644
--- a/mavlink-core/src/connection/direct_serial.rs
+++ b/mavlink-core/src/connection/direct_serial.rs
@@ -7,6 +7,7 @@ use crate::{MavHeader, MavlinkVersion, Message};
 use core::ops::DerefMut;
 use std::io;
 use std::sync::Mutex;
+use std::time::Duration;
 
 use crate::error::{MessageReadError, MessageWriteError};
 use serial::{prelude::*, SystemPort};
@@ -85,6 +86,20 @@ impl<M: Message> MavConnection<M> for SerialConnection {
         self.protocol_version
     }
 
+    fn set_read_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let mut guard = self.port.lock().unwrap();
+        let timeout = timeout.unwrap_or(Duration::ZERO);
+        guard.reader_mut().set_timeout(timeout)?;
+        Ok(())
+    }
+
+    fn set_write_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let mut guard = self.port.lock().unwrap();
+        let timeout = timeout.unwrap_or(Duration::ZERO);
+        guard.reader_mut().set_timeout(timeout)?;
+        Ok(())
+    }
+
     #[cfg(feature = "signing")]
     fn setup_signing(&mut self, signing_data: Option<SigningConfig>) {
         self.signing_data = signing_data.map(SigningData::from_config)
diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs
index 176a4a8be952f39c8db999de8be4060c9812cfa4..0cafa727a7891db74d87d585d82eebd3b65902e5 100644
--- a/mavlink-core/src/connection/file.rs
+++ b/mavlink-core/src/connection/file.rs
@@ -9,6 +9,7 @@ use core::ops::DerefMut;
 use std::fs::File;
 use std::io;
 use std::sync::Mutex;
+use std::time::Duration;
 
 #[cfg(not(feature = "signing"))]
 use crate::read_versioned_msg;
@@ -76,6 +77,20 @@ impl<M: Message> MavConnection<M> for FileConnection {
         self.protocol_version
     }
 
+    fn set_read_timeout(&mut self, _timeout: Option<Duration>) -> io::Result<()> {
+        Err(io::Error::new(
+            io::ErrorKind::Other,
+            "File connection does not support read timeout",
+        ))
+    }
+
+    fn set_write_timeout(&mut self, _timeout: Option<Duration>) -> io::Result<()> {
+        Err(io::Error::new(
+            io::ErrorKind::Other,
+            "File connection does not support write timeout",
+        ))
+    }
+
     #[cfg(feature = "signing")]
     fn setup_signing(&mut self, signing_data: Option<SigningConfig>) {
         self.signing_data = signing_data.map(SigningData::from_config)
diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs
index 5fff34bc7094b7aef3b2fe0494a561c323ec0a7f..4861b01e4efb82a4c34942e241d837407c260ec2 100644
--- a/mavlink-core/src/connection/mod.rs
+++ b/mavlink-core/src/connection/mod.rs
@@ -30,6 +30,9 @@ pub trait MavConnection<M: Message> {
     fn set_protocol_version(&mut self, version: MavlinkVersion);
     fn protocol_version(&self) -> MavlinkVersion;
 
+    fn set_write_timeout(&mut self, timeout: Option<std::time::Duration>) -> io::Result<()>;
+    fn set_read_timeout(&mut self, timeout: Option<std::time::Duration>) -> io::Result<()>;
+
     /// Write whole frame
     fn send_frame(&self, frame: &MavFrame<M>) -> Result<usize, crate::error::MessageWriteError> {
         self.send(&frame.header, &frame.msg)
diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs
index 3f83c2dc628f8c6e7d1e340f73da7f0612267876..b251b368e1eb280a9fae0a79d3cf4dbc57a8a979 100644
--- a/mavlink-core/src/connection/tcp.rs
+++ b/mavlink-core/src/connection/tcp.rs
@@ -126,6 +126,16 @@ impl<M: Message> MavConnection<M> for TcpConnection {
         self.protocol_version
     }
 
+    fn set_read_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let mut guard = self.reader.lock().unwrap();
+        guard.reader_mut().set_read_timeout(timeout)
+    }
+
+    fn set_write_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let guard = self.writer.lock().unwrap();
+        guard.socket.set_write_timeout(timeout)
+    }
+
     #[cfg(feature = "signing")]
     fn setup_signing(&mut self, signing_data: Option<SigningConfig>) {
         self.signing_data = signing_data.map(SigningData::from_config)
diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs
index 25f5662277039ef0091f6712516c69000021afe9..208a4047ff40710ed55c7f22aa206f0be6ae8dcf 100644
--- a/mavlink-core/src/connection/udp.rs
+++ b/mavlink-core/src/connection/udp.rs
@@ -1,6 +1,7 @@
 //! UDP MAVLink connection
 
 use std::collections::VecDeque;
+use std::time::Duration;
 
 use crate::connectable::{UdpConnectable, UdpMode};
 use crate::connection::MavConnection;
@@ -142,6 +143,16 @@ impl<M: Message> MavConnection<M> for UdpConnection {
         self.protocol_version
     }
 
+    fn set_read_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let mut guard = self.reader.lock().unwrap();
+        guard.reader_mut().socket.set_read_timeout(timeout)
+    }
+
+    fn set_write_timeout(&mut self, timeout: Option<Duration>) -> io::Result<()> {
+        let guard = self.writer.lock().unwrap();
+        guard.socket.set_write_timeout(timeout)
+    }
+
     #[cfg(feature = "signing")]
     fn setup_signing(&mut self, signing_data: Option<SigningConfig>) {
         self.signing_data = signing_data.map(SigningData::from_config)