diff --git a/src/main.rs b/src/main.rs
index cd55843..3138387 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -45,7 +45,6 @@ fn main() -> anyhow::Result<()> {
     // threads.push(thread::spawn(move || gps::main(gps_rx, gps_tx, gps_uart, gps_sender)));
     //thread::sleep(Duration::from_millis(1000));
     threads.push(thread::spawn(move || accel::main(accel_sender)));
-    thread::sleep(Duration::from_millis(3000));
 
     let _ = modem::main(modem_rx, modem_tx, modem_uart, modem_pwrkey, modem_rst, modem_power, receiver)?;
 
diff --git a/src/modem.rs b/src/modem.rs
index 1ad179a..9381baf 100644
--- a/src/modem.rs
+++ b/src/modem.rs
@@ -125,12 +125,8 @@ impl<UART: serial::Uart> Modem<UART> {
         println!("-----------------------------------------------------------");
         println!("Sending {} ...", cmd.text);
 
-        let _ = self.serial
-            .write_bytes(cmd.text.as_bytes())
-            .map_err(|_| ModemError::SendDataError(format!("Error in send_command({})", cmd.text)))?;
-
-        let _ = self.serial
-            .write(&['\r' as u8])
+        let _ = nb::block!(self.serial
+            .write_bytes(&[cmd.text.as_bytes(), &['\r' as u8]].concat()))
             .map_err(|_| ModemError::SendDataError(format!("Error in send_command({})", cmd.text)))?;
 
         self.command_read_response(cmd.contains)
@@ -253,7 +249,7 @@ impl<UART: serial::Uart> Modem<UART> {
     pub fn tcp_connect(&mut self, addr: &str, port: u16) -> Result<()> {
         let _ = self.send_command(Command::tcp_connect(addr, port))?;
         for _ in 0..3 {
-            if let Ok(reply) = self.command_read_response(Some("CONNECT_OK".to_string())) {
+            if let Ok(reply) = self.command_read_response(Some("CONNECT OK".to_string())) {
                 println!("TCP connect replied with {}", reply);
                 break
             }
@@ -532,7 +528,11 @@ pub fn main<T: Sync + Send>(
 
             let _ = mdm.tcp_set_quick_mode(false);
             let _ = mdm.tcp_set_manual_receive(true);
-            let _ = mdm.tcp_connect("51.158.66.64", 7887)?;
+
+            if let Err(_) = mdm.tcp_connect("51.158.66.64", 7887) {
+                continue;
+            }
+            thread::sleep(Duration::from_secs(1));
 
             let _ = mdm.mqtt_connect(device_id)?;