diff --git a/src/mavsdk/core/lazy_plugin.h b/src/mavsdk/core/lazy_plugin.h
index 42bf7f3741..c6d9fa2833 100644
--- a/src/mavsdk/core/lazy_plugin.h
+++ b/src/mavsdk/core/lazy_plugin.h
@@ -18,7 +18,7 @@ template<typename Plugin> class LazyPlugin {
             if (_mavsdk.systems().empty()) {
                 return nullptr;
             }
-            _plugin = std::make_unique<Plugin>(_mavsdk.systems()[0]);
+            _plugin = std::make_unique<Plugin>(_mavsdk.first_autopilot(0).value());
         }
         return _plugin.get();
     }
diff --git a/src/mavsdk/core/mocks/system_mock.h b/src/mavsdk/core/mocks/system_mock.h
index 21210c63b4..693a4141b6 100644
--- a/src/mavsdk/core/mocks/system_mock.h
+++ b/src/mavsdk/core/mocks/system_mock.h
@@ -8,6 +8,7 @@ namespace testing {
 class MockSystem {
 public:
     MOCK_CONST_METHOD0(is_connected, bool()){};
+    MOCK_CONST_METHOD0(has_autopilot, bool()){};
 };
 
 } // namespace testing
diff --git a/src/mavsdk_server/src/connection_initiator.h b/src/mavsdk_server/src/connection_initiator.h
index 39d9e6d708..95cfafc798 100644
--- a/src/mavsdk_server/src/connection_initiator.h
+++ b/src/mavsdk_server/src/connection_initiator.h
@@ -57,13 +57,14 @@ template<typename Mavsdk> class ConnectionInitiator {
 
         mavsdk.subscribe_on_new_system([this, &mavsdk]() {
             std::lock_guard<std::mutex> guard(_mutex);
-            const auto system = mavsdk.systems().at(0);
-
-            if (!_is_discovery_finished && system->is_connected()) {
-                LogInfo() << "System discovered";
-
-                _is_discovery_finished = true;
-                _discovery_promise->set_value(true);
+            for (auto system : mavsdk.systems()) {
+                if (!_is_discovery_finished && system->has_autopilot() && system->is_connected()) {
+                    LogInfo() << "System discovered";
+
+                    _is_discovery_finished = true;
+                    _discovery_promise->set_value(true);
+                    break;
+                }
             }
         });
 
diff --git a/src/mavsdk_server/test/connection_initiator_test.cpp b/src/mavsdk_server/test/connection_initiator_test.cpp
index 2f4af6c2ae..fcca8b3a42 100644
--- a/src/mavsdk_server/test/connection_initiator_test.cpp
+++ b/src/mavsdk_server/test/connection_initiator_test.cpp
@@ -54,6 +54,7 @@ TEST(ConnectionInitiator, startHangsUntilSystemDiscovered)
     EXPECT_CALL(mavsdk, systems()).WillOnce(testing::Return(systems));
 
     EXPECT_CALL(*system, is_connected()).WillOnce(testing::Return(true));
+    EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true));
 
     auto async_future = std::async(std::launch::async, [&initiator, &mavsdk]() {
         initiator.start(mavsdk, ARBITRARY_CONNECTION_URL);
@@ -80,6 +81,7 @@ TEST(ConnectionInitiator, connectionDetectedIfDiscoverCallbackCalledBeforeWait)
     EXPECT_CALL(mavsdk, systems()).WillOnce(testing::Return(systems));
 
     EXPECT_CALL(*system, is_connected()).WillOnce(testing::Return(true));
+    EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true));
 
     initiator.start(mavsdk, ARBITRARY_CONNECTION_URL);
     change_callback();
@@ -99,6 +101,7 @@ TEST(ConnectionInitiator, doesNotCrashIfDiscoverCallbackCalledMoreThanOnce)
     EXPECT_CALL(mavsdk, systems()).WillRepeatedly(testing::Return(systems));
 
     EXPECT_CALL(*system, is_connected()).WillRepeatedly(testing::Return(true));
+    EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true));
 
     initiator.start(mavsdk, ARBITRARY_CONNECTION_URL);
     change_callback();