diff --git a/src/serial.rs b/src/serial.rs
index 4011d74d0a25b33b8aed28c28dee9545ef8e21f4..cdaf70095e72cc140878b876b1278695c427f9e3 100644
--- a/src/serial.rs
+++ b/src/serial.rs
@@ -23,8 +23,8 @@ pub struct SerialManager {
 /// Queue data to allow non-blocking writes and blocking reads
 #[derive(Debug)]
 struct SerialQueue {
-    _handle: JoinHandle<()>,
-    sender: Sender<Vec<u8>>,
+    handle: Option<JoinHandle<()>>,
+    sender: Option<Sender<Vec<u8>>>,
     serial: Arc<Mutex<Box<dyn SerialPort>>>,
     /// this the timeout after which it returns a timeout error
     read_timeout: Duration,
@@ -125,14 +125,14 @@ impl SerialQueue {
         ));
         let (tx, rx) = channel::<Vec<u8>>();
         let ser = Arc::clone(&serial);
-        let _handle = thread::spawn(move || {
+        let handle = thread::spawn(move || {
             while let Ok(data) = rx.recv() {
                 ser.lock().write_all(&data).unwrap();
             }
         });
         Ok(Self {
-            _handle,
-            sender: tx,
+            handle: Some(handle),
+            sender: Some(tx),
             serial,
             read_timeout: Duration::from_millis(1000),
         })
@@ -140,7 +140,14 @@ impl SerialQueue {
 
     /// Enqueue `data` to be written to the serial port, this is non-blocking
     fn enqueue_bytes(&self, data: &[u8]) -> SResult<()> {
-        self.sender.send(data.to_vec())?;
+        self.sender.as_ref().unwrap().send(data.to_vec())?;
         Ok(())
     }
 }
+
+impl Drop for SerialQueue {
+    fn drop(&mut self) {
+        drop(self.sender.take().unwrap());
+        self.handle.take().unwrap().join().unwrap();
+    }
+}