Call _new_system_callback on subscribe

mavlink · Dec 16, 2020 · fbc54ea · fbc54ea
1 parent faecdf8
commit fbc54ea
Showing 1 changed file with 20 additions and 10 deletions.
diff --git 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