mavsdk_server: default to the first autopilot (instead of the first s…

…ystem)
mavlink · Oct 19, 2023 · e9402ae · e9402ae
1 parent 441ba3a
commit e9402ae
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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;
+                }
             }
         });