From e9402ae7e120e30136d46361349ff5c4fe1046f7 Mon Sep 17 00:00:00 2001
From: Jonas Vautherin <dev@jonas.vautherin.ch>
Date: Thu, 19 Oct 2023 23:16:15 +0200
Subject: [PATCH 1/2] mavsdk_server: default to the first autopilot (instead of
 the first system)

---
 src/mavsdk/core/lazy_plugin.h                | 15 ++++++++++++++-
 src/mavsdk/core/mocks/system_mock.h          |  1 +
 src/mavsdk_server/src/connection_initiator.h | 15 ++++++++-------
 3 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/src/mavsdk/core/lazy_plugin.h b/src/mavsdk/core/lazy_plugin.h
index 42bf7f3741..688aa06c9a 100644
--- a/src/mavsdk/core/lazy_plugin.h
+++ b/src/mavsdk/core/lazy_plugin.h
@@ -4,6 +4,7 @@
 #include <mutex>
 
 #include <mavsdk.h>
+#include <log.h>
 
 namespace mavsdk::mavsdk_server {
 
@@ -18,7 +19,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>(first_autopilot());
         }
         return _plugin.get();
     }
@@ -27,6 +28,18 @@ template<typename Plugin> class LazyPlugin {
     Mavsdk& _mavsdk;
     std::unique_ptr<Plugin> _plugin{};
     std::mutex _mutex{};
+
+    std::shared_ptr<System> first_autopilot()
+    {
+        for (auto system : _mavsdk.systems()) {
+            if (system->has_autopilot()) {
+                return system;
+            }
+        }
+
+        LogErr() << "No autopilot found!";
+        return nullptr;
+    }
 };
 
 } // namespace mavsdk::mavsdk_server
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;
+                }
             }
         });
 

From 494caa87d65aaf25fc2c69db1a43ead4bb24e8c1 Mon Sep 17 00:00:00 2001
From: Jonas Vautherin <dev@jonas.vautherin.ch>
Date: Fri, 20 Oct 2023 00:49:31 +0200
Subject: [PATCH 2/2] mavsdk_server: fix unit tests

---
 src/mavsdk_server/test/connection_initiator_test.cpp | 3 +++
 1 file changed, 3 insertions(+)

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();