Merge pull request #2164 from mavlink/mavsdk-server-default-to-autopilot

Mavsdk server default to autopilot
mavlink · Oct 21, 2023 · b29239d · b29239d
2 parents a9c1981 + edf5960
commit b29239d
Show file tree

Hide file tree

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

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