diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h
index f9e5399..5eb90a9 100644
--- a/include/mavlink_interface.h
+++ b/include/mavlink_interface.h
@@ -202,7 +202,7 @@ class MavlinkInterface {
     struct pollfd fds_[N_FDS];
     bool use_tcp_{false};
     bool tcp_client_mode_{false};
-    bool close_conn_{false};
+    std::atomic<bool> close_conn_{false};
 
     in_addr_t mavlink_addr_;
     std::string mavlink_addr_str_{"INADDR_ANY"};
@@ -230,18 +230,18 @@ class MavlinkInterface {
     std::atomic<bool> tx_in_progress_;
     std::deque<MsgBuffer> tx_q_{};
 
-    bool baro_updated_;
-    bool diff_press_updated_;
-    bool mag_updated_;
-    bool imu_updated_;
-
-    double temperature_;
-    double pressure_alt_;
-    double abs_pressure_;
-    double diff_pressure_;
-    Eigen::Vector3d mag_b_;
-    Eigen::Vector3d accel_b_;
-    Eigen::Vector3d gyro_b_;
+    bool baro_updated_{};
+    bool diff_press_updated_{};
+    bool mag_updated_{};
+    bool imu_updated_{};
+
+    double temperature_{};
+    double pressure_alt_{};
+    double abs_pressure_{};
+    double diff_pressure_{};
+    Eigen::Vector3d mag_b_{};
+    Eigen::Vector3d accel_b_{};
+    Eigen::Vector3d gyro_b_{};
 
     //std::vector<HILData, Eigen::aligned_allocator<HILData>> hil_data_;
     std::atomic<bool> gotSigInt_ {false};
diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp
index bff423f..f87bca3 100644
--- a/src/mavlink_interface.cpp
+++ b/src/mavlink_interface.cpp
@@ -216,7 +216,7 @@ void MavlinkInterface::ReceiveWorker() {
       }
     }
   }
-  std::cout << "[" << thrd_name << "] shutdown" << std::endl;
+  std::cout << "The thread [" << thrd_name << "] was shutdown." << std::endl;
 
 }
 
@@ -264,8 +264,10 @@ void MavlinkInterface::SendWorker() {
       return close_conn_ || gotSigInt_ || !sender_buffer_.empty();
     });
 
-    std::shared_ptr<mavlink_message_t> msg;
-    msg = sender_buffer_.front();
+    if (sender_buffer_.empty())
+      continue;
+
+    auto msg = sender_buffer_.front();
     if (msg) {
       sender_buffer_.pop();
       lock.unlock();
@@ -275,7 +277,7 @@ void MavlinkInterface::SendWorker() {
     }
   }
 
-  std::cout << "[" << thrd_name << "] Shutdown.." << std::endl;
+  std::cout << "The thread [" << thrd_name << "] was shutdown." << std::endl;
 }
 
 void MavlinkInterface::SendSensorMessages(uint64_t time_usec) {