diff --git a/src/core/mavsdk_impl.cpp b/src/core/mavsdk_impl.cpp
index 9291de0606..986dee3db9 100644
--- a/src/core/mavsdk_impl.cpp
+++ b/src/core/mavsdk_impl.cpp
@@ -452,6 +452,12 @@ void MavsdkImpl::notify_on_timeout(const uint64_t uuid)
     if (_on_timeout_callback) {
         _on_timeout_callback(uuid);
     }
+
+    std::lock_guard<std::mutex> lock(_new_system_callback_mutex);
+    if (_new_system_callback) {
+        auto temp_callback = _new_system_callback;
+        call_user_callback([temp_callback]() { temp_callback(); });
+    }
 }
 
 void MavsdkImpl::subscribe_on_new_system(Mavsdk::NewSystemCallback callback)
diff --git a/src/core/tcp_connection.cpp b/src/core/tcp_connection.cpp
index 11043f0d6d..a892e81ed5 100644
--- a/src/core/tcp_connection.cpp
+++ b/src/core/tcp_connection.cpp
@@ -130,6 +130,10 @@ ConnectionResult TcpConnection::stop()
 
 bool TcpConnection::send_message(const mavlink_message_t& message)
 {
+    if (!_is_ok) {
+        return false;
+    }
+
     if (_remote_ip.empty()) {
         LogErr() << "Remote IP unknown";
         return false;
@@ -153,11 +157,17 @@ bool TcpConnection::send_message(const mavlink_message_t& message)
     // TODO: remove this assert again
     assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);
 
+#if defined(WINDOWS)
+    auto flags = 0;
+#else
+    auto flags = MSG_NOSIGNAL;
+#endif
+
     const auto send_len = sendto(
         _socket_fd,
         reinterpret_cast<char*>(buffer),
         buffer_len,
-        0,
+        flags,
         reinterpret_cast<const sockaddr*>(&dest_addr),
         sizeof(dest_addr));
 
diff --git a/src/mavsdk_server/src/core/core_service_impl.h b/src/mavsdk_server/src/core/core_service_impl.h
index 4cfd0949af..18b868e708 100644
--- a/src/mavsdk_server/src/core/core_service_impl.h
+++ b/src/mavsdk_server/src/core/core_service_impl.h
@@ -23,18 +23,13 @@ class CoreServiceImpl final : public mavsdk::rpc::core::CoreService::Service {
     {
         std::mutex connection_state_mutex{};
 
-        _mavsdk.subscribe_on_new_system([this, &writer, &connection_state_mutex]() {
-            auto systems = _mavsdk.systems();
-
-            for (auto system : systems) {
-                const auto rpc_connection_state_response =
-                    createRpcConnectionStateResponse(system->is_connected());
-
-                std::lock_guard<std::mutex> lock(connection_state_mutex);
-                writer->Write(rpc_connection_state_response);
-            }
+        _mavsdk.subscribe_on_new_system([this, writer, &connection_state_mutex]() {
+            publish_system_state(writer, connection_state_mutex);
         });
 
+        // Publish the current state on subscribe
+        publish_system_state(writer, connection_state_mutex);
+
         _stop_future.wait();
         return grpc::Status::OK;
     }
@@ -88,6 +83,21 @@ class CoreServiceImpl final : public mavsdk::rpc::core::CoreService::Service {
 
         return rpc_connection_state_response;
     }
+
+    void publish_system_state(
+        grpc::ServerWriter<rpc::core::ConnectionStateResponse>* writer,
+        std::mutex& connection_state_mutex)
+    {
+        auto systems = _mavsdk.systems();
+
+        for (auto system : systems) {
+            const auto rpc_connection_state_response =
+                createRpcConnectionStateResponse(system->is_connected());
+
+            std::lock_guard<std::mutex> lock(connection_state_mutex);
+            writer->Write(rpc_connection_state_response);
+        }
+    }
 };
 
 } // namespace mavsdk_server