Merge pull request #1418 from mavlink/udp-client-connection

Initiate UDP connection from MAVSDK

.github/workflows/main.yml

@@ -90,6 +90,8 @@ jobs:
         run: ./build/release/src/unit_tests_runner
       - name: test (mavsdk_server)
         run: ./build/release/src/mavsdk_server/test/unit_tests_mavsdk_server
+      - name: test FTP server
+        run: ./build/release/src/integration_tests/integration_tests_runner --gtest_filter="FtpTest.TestServer"
 
   ubuntu20-proto-check:
     name: ubuntu-20.04 (proto check)

src/core/mavsdk_impl.cpp

@@ -275,15 +275,15 @@ ConnectionResult MavsdkImpl::add_any_connection(
 
     switch (cli_arg.get_protocol()) {
         case CliArg::Protocol::Udp: {
-            std::string path = Mavsdk::DEFAULT_UDP_BIND_IP;
-            int port = Mavsdk::DEFAULT_UDP_PORT;
-            if (!cli_arg.get_path().empty()) {
-                path = cli_arg.get_path();
-            }
-            if (cli_arg.get_port()) {
-                port = cli_arg.get_port();
+            int port = cli_arg.get_port() ? cli_arg.get_port() : Mavsdk::DEFAULT_UDP_PORT;
+
+            if (cli_arg.get_path().empty() || cli_arg.get_path() == Mavsdk::DEFAULT_UDP_BIND_IP) {
+                std::string path = Mavsdk::DEFAULT_UDP_BIND_IP;
+                return add_udp_connection(path, port, forwarding_option);
+            } else {
+                std::string path = cli_arg.get_path();
+                return setup_udp_remote(path, port, forwarding_option);
             }
-            return add_udp_connection(path, port, forwarding_option);
         }
 
         case CliArg::Protocol::Tcp: {
@@ -346,7 +346,7 @@ ConnectionResult MavsdkImpl::setup_udp_remote(
     if (ret == ConnectionResult::Success) {
         new_conn->add_remote(remote_ip, remote_port);
         add_connection(new_conn);
-        make_system_with_component(get_own_system_id(), get_own_component_id(), true);
+        make_system_with_component(0, 0, true);
     }
     return ret;
 }

src/core/system_impl.cpp

@@ -195,14 +195,13 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
         }
     }
 
-    // We do not call on_discovery here but wait with the notification until we know the UUID.
-
     // If the component is an autopilot and we don't know its UUID, then try to find out.
-    if (is_autopilot(message.compid) && !have_uuid()) {
+    // If it's an autopilot, set_connected() will be called in process_autopilot_version().
+    if (is_autopilot(message.compid) && !_uuid_initialized) {
         request_autopilot_version();
+    } else {
+        set_connected();
     }
-
-    set_connected();
 }
 
 void SystemImpl::process_autopilot_version(const mavlink_message_t& message)
@@ -508,7 +507,7 @@ void SystemImpl::set_connected()
     {
         std::lock_guard<std::mutex> lock(_connection_mutex);
 
-        if (!_connected && _uuid_initialized) {
+        if (!_connected) {
             LogDebug() << "Discovered " << _components.size() << " component(s) "
                        << "(UUID: " << _uuid << ")";
 

src/integration_tests/CMakeLists.txt

@@ -37,6 +37,7 @@ add_executable(integration_tests_runner
     camera_take_photo_interval.cpp
     camera_format.cpp
     camera_test_helpers.cpp
+    connection.cpp
     follow_me.cpp
     geofence_inclusion.cpp
     gimbal.cpp

src/integration_tests/connection.cpp

@@ -0,0 +1,100 @@
+#include <chrono>
+#include <future>
+#include <gtest/gtest.h>
+#include <iostream>
+
+#include "mavsdk.h"
+#include "plugins/action/action.h"
+#include "plugins/mavlink_passthrough/mavlink_passthrough.h"
+
+using namespace mavsdk;
+
+static void
+connection_test(const std::string& client_system_address, const std::string& server_system_address);
+static std::shared_ptr<System> discover_system(Mavsdk& mavsdk_instance);
+static void prepare_autopilot(MavlinkPassthrough& mavlink_passthrough);
+
+TEST(ConnectionTest, UdpListenOnDefaultPath)
+{
+    connection_test("udp://127.0.0.1:55555", "udp://:55555");
+}
+
+TEST(ConnectionTest, UdpListenOnExplicitPath)
+{
+    connection_test("udp://127.0.0.1:55555", "udp://0.0.0.0:55555");
+}
+
+static void
+connection_test(const std::string& client_system_address, const std::string& server_system_address)
+{
+    // Start instance as a UDP server pretending to be an autopilot
+    Mavsdk mavsdk_server;
+    Mavsdk::Configuration config_autopilot(Mavsdk::Configuration::UsageType::Autopilot);
+    mavsdk_server.set_configuration(config_autopilot);
+    ConnectionResult ret_server = mavsdk_server.add_any_connection(server_system_address);
+    ASSERT_EQ(ret_server, ConnectionResult::Success);
+
+    // Start instance as a UDP client, connecting to the server above
+    Mavsdk mavsdk_client;
+    ConnectionResult ret_client = mavsdk_client.add_any_connection(client_system_address);
+    ASSERT_EQ(ret_client, ConnectionResult::Success);
+
+    // Wait for autopilot to discover client
+    auto autopilot_to_client_system = discover_system(mavsdk_server);
+
+    // Wait for client to discover autopilot
+    auto client_to_autopilot_system = discover_system(mavsdk_client);
+
+    // Prepare autopilot to ACK any command it receives
+    MavlinkPassthrough mavlink_passthrough(autopilot_to_client_system);
+    prepare_autopilot(mavlink_passthrough);
+
+    // Send any command to the autopilot (which should ACK)
+    Action action(client_to_autopilot_system);
+    Action::Result takeoff_result = action.takeoff();
+
+    EXPECT_EQ(takeoff_result, Action::Result::Success);
+}
+
+static std::shared_ptr<System> discover_system(Mavsdk& mavsdk_instance)
+{
+    std::promise<void> prom;
+    std::future<void> fut = prom.get_future();
+    mavsdk_instance.subscribe_on_new_system([&mavsdk_instance, &prom]() {
+        const auto system = mavsdk_instance.systems().at(0);
+
+        if (system->is_connected()) {
+            prom.set_value();
+        }
+    });
+
+    EXPECT_EQ(fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);
+
+    return mavsdk_instance.systems().at(0);
+}
+
+static void prepare_autopilot(MavlinkPassthrough& mavlink_passthrough)
+{
+    mavlink_passthrough.subscribe_message_async(
+        MAVLINK_MSG_ID_COMMAND_LONG,
+        [&mavlink_passthrough](const mavlink_message_t& mavlink_message) {
+            mavlink_command_long_t cmd_read;
+            mavlink_msg_command_long_decode(&mavlink_message, &cmd_read);
+
+            const auto cmd_id = cmd_read.command;
+            auto mav_result = MAV_RESULT_ACCEPTED;
+
+            mavlink_message_t message;
+            mavlink_msg_command_ack_pack(
+                mavlink_passthrough.get_our_sysid(),
+                mavlink_passthrough.get_our_compid(),
+                &message,
+                cmd_id,
+                mav_result,
+                255,
+                -1,
+                mavlink_passthrough.get_target_sysid(),
+                mavlink_passthrough.get_target_compid());
+            mavlink_passthrough.send_message(message);
+        });
+}

src/integration_tests/ftp.cpp

@@ -275,20 +275,19 @@ TEST(FtpTest, UploadFiles)
 
 TEST(FtpTest, TestServer)
 {
-    ConnectionResult ret;
-
     Mavsdk mavsdk_gcs;
     Mavsdk::Configuration config_gcs(Mavsdk::Configuration::UsageType::GroundStation);
     mavsdk_gcs.set_configuration(config_gcs);
-    ret = mavsdk_gcs.add_udp_connection(24550);
-    ASSERT_EQ(ret, ConnectionResult::Success);
-    auto system_gcs = mavsdk_gcs.systems().at(0);
+    ASSERT_EQ(mavsdk_gcs.add_any_connection("udp://:24550"), ConnectionResult::Success);
 
     Mavsdk mavsdk_cc;
     Mavsdk::Configuration config_cc(Mavsdk::Configuration::UsageType::GroundStation);
     mavsdk_cc.set_configuration(config_cc);
-    ret = mavsdk_cc.setup_udp_remote("127.0.0.1", 24550);
-    ASSERT_EQ(ret, ConnectionResult::Success);
+    ASSERT_EQ(mavsdk_cc.add_any_connection("udp://127.0.0.1:24550"), ConnectionResult::Success);
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    auto system_gcs = mavsdk_gcs.systems().at(0);
     auto system_cc = mavsdk_cc.systems().at(0);
 
     auto mavlink_passthrough_cc = std::make_shared<MavlinkPassthrough>(system_cc);