diff --git a/src/main.rs b/src/main.rs
index cec2efa69d40441fc24b531f3ac16babaa0ecba4..1f34d6d9dcb9dbfb903fd19841abbd862c4ac197 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -5,6 +5,18 @@ use std::time::Duration;
 use std::{io, thread};
 
 use ffi::MavlinkPayloadFlightTM;
+use serialport::SerialPort;
+
+trait MavLinkPort {
+    fn write_packet(&mut self, packet: MavlinkPayloadFlightTM);
+}
+
+impl<T: AsRef<dyn SerialPort> + std::io::Write> MavLinkPort for T {
+    fn write_packet(&mut self, packet: MavlinkPayloadFlightTM) {
+        self.write_all(&packet.into_le_bytes())
+            .expect("Failed to write to serial port");
+    }
+}
 
 fn main() {
     // Open the first serialport available.
@@ -12,27 +24,20 @@ fn main() {
     let mut port = serialport::new(port_name, 115200)
         .open()
         .expect("Failed to open serial port");
+    let mut write_port = port.try_clone().expect("Failed to clone");
 
-    // Clone the port
-    let mut clone = port.try_clone().expect("Failed to clone");
-
-    // Send out 4 bytes every second
-    thread::spawn(move || {
-        thread::sleep(Duration::from_millis(2000));
-        let mav = MavlinkPayloadFlightTM::default();
-
-        clone
-            // .write_all(&send_buffer)
-            .write_all(&mav.into_le_bytes())
-            .expect("Failed to write to serial port");
-    });
-
-    // Read the four bytes back from the cloned port
+    // read from buffer and print to stdout
     let mut buffer = [0u8; 1000];
     loop {
         match port.read(&mut buffer) {
             Ok(t) => {
-                io::stdout().write_all(&buffer[..t]).unwrap();
+                // encode the buffer into a string
+                let input = String::from_utf8_lossy(&buffer[..t]);
+                // if we got the signal to start sending data, send it
+                if input.contains("[WAITING]") {
+                    write_port.write_packet(MavlinkPayloadFlightTM::default());
+                }
+                io::stdout().write_all(input.as_bytes()).unwrap();
             }
             Err(ref e) if e.kind() == io::ErrorKind::TimedOut => (),
             Err(e) => eprintln!("{:?}", e),