From e8891889d62b6d50ac413adc58fc27f9099a0dc9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Jun 2024 20:07:45 +1200 Subject: [PATCH 01/17] core: switch to add_any_connection, add in/out This removes the add_udp/tcp/serial_connection and instead switches to only add_any_connection. Also, we now encourage the use of udpin and udpout instead of udp only to make it clear what is connecting like a client or server. --- src/mavsdk/core/cli_arg.cpp | 342 +++++++++++++------- src/mavsdk/core/cli_arg.h | 59 ++-- src/mavsdk/core/cli_arg_test.cpp | 402 ++++++++++++++++++------ src/mavsdk/core/include/mavsdk/mavsdk.h | 143 +++------ src/mavsdk/core/mavsdk.cpp | 60 +--- src/mavsdk/core/mavsdk_impl.cpp | 66 ++-- src/mavsdk/core/mavsdk_impl.h | 45 +-- 7 files changed, 671 insertions(+), 446 deletions(-) diff --git a/src/mavsdk/core/cli_arg.cpp b/src/mavsdk/core/cli_arg.cpp index 3b741966a0..7626102379 100644 --- a/src/mavsdk/core/cli_arg.cpp +++ b/src/mavsdk/core/cli_arg.cpp @@ -4,172 +4,284 @@ #include #include #include +#include +#include namespace mavsdk { -void CliArg::reset() -{ - _protocol = Protocol::None; - _path.clear(); - _baudrate = 0; - _port = 0; -} - bool CliArg::parse(const std::string& uri) { - reset(); + // Reset + protocol = {}; - std::string rest(uri); - if (!find_protocol(rest)) { - return false; + const std::string udp = "udp"; + const std::string udpin = "udpin"; + const std::string udpout = "udpout"; + const std::string tcp = "tcp"; + const std::string tcpin = "tcpin"; + const std::string tcpout = "tcpout"; + const std::string serial = "serial"; + const std::string serial_flowcontrol = "serial_flowcontrol"; + const std::string delimiter = "://"; + + if (uri.find(udp + delimiter) == 0) { + return parse_udp(uri.substr(udp.size() + delimiter.size())); } - if (!find_path(rest)) { - return false; + if (uri.find(udpin + delimiter) == 0) { + return parse_udpin(uri.substr(udpin.size() + delimiter.size())); } - if (_protocol == Protocol::Serial) { - if (!find_baudrate(rest)) { - return false; - } - } else { - if (!find_port(rest)) { - return false; - } + if (uri.find(udpout + delimiter) == 0) { + return parse_udpout(uri.substr(udpout.size() + delimiter.size())); } - return true; + if (uri.find(tcp + delimiter) == 0) { + return parse_tcp(uri.substr(tcp.size() + delimiter.size())); + } + + if (uri.find(tcpin + delimiter) == 0) { + return parse_tcpin(uri.substr(tcpin.size() + delimiter.size())); + } + + if (uri.find(tcpout + delimiter) == 0) { + return parse_tcpout(uri.substr(tcpout.size() + delimiter.size())); + } + + if (uri.find(serial + delimiter) == 0) { + return parse_serial(uri.substr(serial.size() + delimiter.size()), false); + } + + if (uri.find(serial_flowcontrol + delimiter) == 0) { + return parse_serial(uri.substr(serial_flowcontrol.size() + delimiter.size()), true); + } + + LogErr() << "Unknown protocol"; + return false; } -bool CliArg::find_protocol(std::string& rest) +bool CliArg::parse_udp(const std::string_view rest) { - const std::string udp = "udp"; - const std::string tcp = "tcp"; - const std::string serial = "serial"; - const std::string serial_flowcontrol = "serial_flowcontrol"; - const std::string delimiter = "://"; + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; + } - if (rest.find(udp + delimiter) == 0) { - _protocol = Protocol::Udp; - rest.erase(0, udp.length() + delimiter.length()); - return true; - } else if (rest.find(tcp + delimiter) == 0) { - _protocol = Protocol::Tcp; - rest.erase(0, tcp.length() + delimiter.length()); - return true; - } else if (rest.find(serial + delimiter) == 0) { - _protocol = Protocol::Serial; - _flow_control_enabled = false; - rest.erase(0, serial.length() + delimiter.length()); - return true; - } else if (rest.find(serial_flowcontrol + delimiter) == 0) { - _protocol = Protocol::Serial; - _flow_control_enabled = true; - rest.erase(0, serial_flowcontrol.length() + delimiter.length()); + protocol = Udp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + if (p->host.empty()) { + p->host = "0.0.0.0"; + p->mode = Udp::Mode::In; + } else if (p->host == "0.0.0.0") { + p->mode = Udp::Mode::In; + } else { + p->mode = Udp::Mode::Out; + } + + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); return true; } else { - LogWarn() << "Unknown protocol"; return false; } } -bool CliArg::find_path(std::string& rest) +bool CliArg::parse_udpin(const std::string_view rest) { - if (rest.length() == 0) { - if (_protocol == Protocol::Udp || _protocol == Protocol::Tcp) { - // We have to use the default path - return true; - } else { - LogWarn() << "Path for serial device required."; - return false; - } + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; + } + + protocol = Udp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + p->mode = Udp::Mode::In; + + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); + return true; + } else { + return false; } +} +bool CliArg::parse_udpout(const std::string_view rest) +{ const std::string delimiter = ":"; size_t pos = rest.find(delimiter); - if (pos != std::string::npos) { - _path = rest.substr(0, pos); - rest.erase(0, pos + delimiter.length()); + if (pos == std::string::npos) { + return false; + } + + protocol = Udp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + p->mode = Udp::Mode::Out; + + if (p->host == "0.0.0.0") { + LogErr() << "0.0.0.0 is invalid for UDP out address. " + "Can only listen on all interfaces, but not send."; + return false; + } + + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); + return true; } else { - const auto path_is_only_numbers = std::all_of( - rest.cbegin(), rest.cend(), [](unsigned char c) { return std::isdigit(c); }); + return false; + } +} - if (path_is_only_numbers) { - LogWarn() << "Path can't be numbers only."; - return false; - } else { - _path = rest; - rest = ""; - } +bool CliArg::parse_tcp(const std::string_view rest) +{ + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; } - if (_protocol == Protocol::Serial) { - if (_path.find('/') == 0) { - // A Linux/macOS path starting with '/' is ok. - return true; - } else if (_path.find("COM") == 0) { - // On Windows a path starting with 'COM' is ok but needs to be followed by digits. - if (_path.length() == 3) { - LogWarn() << "COM port number missing"; - return false; - } - for (const auto& digit : _path.substr(3, _path.length() - 3)) { - if (!std::isdigit(digit)) { - LogWarn() << "COM port number invalid."; - _path = ""; - return false; - } - } - } else { - LogWarn() << "Invalid serial path"; - _path = ""; - return false; - } + protocol = Tcp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + if (p->host.empty()) { + p->host = "0.0.0.0"; + p->mode = Tcp::Mode::In; + } else if (p->host == "0.0.0.0") { + p->mode = Tcp::Mode::In; + } else { + p->mode = Tcp::Mode::Out; } - return true; + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); + return true; + } else { + return false; + } } -bool CliArg::find_port(std::string& rest) +bool CliArg::parse_tcpin(const std::string_view rest) { - if (rest.length() == 0) { - _port = 0; + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; + } + + protocol = Tcp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + p->mode = Tcp::Mode::In; + + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); return true; + } else { + return false; } +} - for (const auto& digit : rest) { - if (!std::isdigit(digit)) { - LogWarn() << "Non-numeric char found in port"; - return false; - } +bool CliArg::parse_tcpout(const std::string_view rest) +{ + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; } - _port = std::stoi(rest); - if (_port < 0) { - LogWarn() << "Port can't be negative."; - _port = 0; + + protocol = Tcp{}; + auto p = std::get_if(&protocol); + p->host = rest.substr(0, pos); + p->mode = Tcp::Mode::Out; + + if (p->host == "0.0.0.0") { + LogErr() << "0.0.0.0 is invalid for TCP out address. " + "Can only listen on all interfaces, but not send."; return false; - } else if (_port > std::numeric_limits::max()) { - LogWarn() << "Port number to big."; - _port = 0; + } + + if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { + p->port = maybe_port.value(); + return true; + } else { return false; } - return true; } -bool CliArg::find_baudrate(std::string& rest) +std::optional CliArg::port_from_str(std::string_view str) { - if (rest.length() == 0) { - _port = 0; - return true; + int value; + auto [ptr, ec] = std::from_chars(str.data(), str.data() + str.size(), value); + + if (static_cast(ec) || value < 1 || value > 65535) { + return {}; } + return {value}; +} + +bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled) +{ + protocol = Serial{}; + auto p = std::get_if(&protocol); + p->flow_control_enabled = flow_control_enabled; - for (const auto& digit : rest) { - if (!std::isdigit(digit)) { - LogWarn() << "Non-numeric char found in baudrate"; + const std::string delimiter = ":"; + size_t pos = rest.find(delimiter); + if (pos == std::string::npos) { + return false; + } + + p->path = rest.substr(0, pos); + + const auto path_is_only_numbers = std::all_of( + p->path.begin(), p->path.end(), [](unsigned char c) { return std::isdigit(c); }); + + if (path_is_only_numbers) { + LogErr() << "Path can't be numbers only."; + return false; + } + + if (p->path.find('/') == 0) { + // A Linux/macOS path needs to start with '/'. + } else if (p->path.find("COM") == 0) { + // On Windows a path starting with 'COM' is ok but needs to be followed by digits. + for (const auto& digit : p->path.substr(3, p->path.length() - 3)) { + if (!std::isdigit(digit)) { + LogErr() << "COM port number invalid."; + return false; + } + } + + if (p->path.length() == 3) { + LogErr() << "COM port number missing"; return false; } + } else { + LogErr() << "serial port needs to start with / or COM on Windows"; + return false; + } + + auto baudrate_str = rest.substr(pos + 1); + + int value; + auto [ptr, ec] = + std::from_chars(baudrate_str.data(), baudrate_str.data() + baudrate_str.size(), value); + + if (static_cast(ec)) { + return {}; + } + + if (value < 0) { + LogErr() << "Baudrate can't be negative."; + return false; } - _baudrate = std::stoi(rest); + + p->baudrate = value; + return true; } diff --git a/src/mavsdk/core/cli_arg.h b/src/mavsdk/core/cli_arg.h index 29e6cc83ac..8ee0117952 100644 --- a/src/mavsdk/core/cli_arg.h +++ b/src/mavsdk/core/cli_arg.h @@ -1,37 +1,58 @@ #pragma once +#include #include +#include +#include namespace mavsdk { class CliArg { public: - enum class Protocol { None, Udp, Tcp, Serial }; + struct Udp { + enum class Mode { + Unknown, + In, + Out, + } mode{Mode::Unknown}; + std::string host{}; + int port{}; + }; + + struct Tcp { + enum class Mode { + Unknown, + In, + Out, + } mode{Mode::Unknown}; + std::string host{}; + int port{}; + }; + + struct Serial { + std::string path{}; + int baudrate{}; + bool flow_control_enabled{false}; + }; + + using Protocol = std::variant; bool parse(const std::string& uri); - [[nodiscard]] Protocol get_protocol() const { return _protocol; } + Protocol protocol{}; - [[nodiscard]] int get_port() const { return _port; } - - [[nodiscard]] int get_baudrate() const { return _baudrate; } +private: + static std::optional port_from_str(std::string_view str); - [[nodiscard]] bool get_flow_control() const { return _flow_control_enabled; } + bool parse_udp(const std::string_view rest); + bool parse_udpin(const std::string_view rest); + bool parse_udpout(const std::string_view rest); - [[nodiscard]] std::string get_path() const { return _path; } + bool parse_tcp(const std::string_view rest); + bool parse_tcpin(const std::string_view rest); + bool parse_tcpout(const std::string_view rest); -private: - void reset(); - bool find_protocol(std::string& rest); - bool find_path(std::string& rest); - bool find_port(std::string& rest); - bool find_baudrate(std::string& rest); - - Protocol _protocol{Protocol::None}; - std::string _path{}; - int _port{0}; - int _baudrate{0}; - bool _flow_control_enabled{false}; + bool parse_serial(const std::string_view rest, bool flow_control_enabled); }; } // namespace mavsdk diff --git a/src/mavsdk/core/cli_arg_test.cpp b/src/mavsdk/core/cli_arg_test.cpp index 864ebe83c1..a8eb7fa901 100644 --- a/src/mavsdk/core/cli_arg_test.cpp +++ b/src/mavsdk/core/cli_arg_test.cpp @@ -4,54 +4,106 @@ using namespace mavsdk; -TEST(CliArg, UDPConnections) +TEST(CliArg, NothingInitially) { CliArg ca; - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::None); - - ca.parse("udp://127.0.0.1"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "127.0.0.1"); - EXPECT_EQ(0, ca.get_port()); + EXPECT_TRUE(std::get_if(&ca.protocol)); +} - ca.parse("udp://"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), ""); - EXPECT_EQ(0, ca.get_port()); +TEST(CliArg, UdpLegacyNoPort) +{ + CliArg ca; + EXPECT_FALSE(ca.parse("udp://127.0.0.1")); + EXPECT_TRUE(std::get_if(&ca.protocol)); +} - // Not a valid hostname +TEST(CliArg, UdpLegacyNoIp) +{ + CliArg ca; EXPECT_FALSE(ca.parse("udp://555")); + EXPECT_TRUE(std::get_if(&ca.protocol)); +} - ca.parse("udp://:777"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), ""); - EXPECT_EQ(777, ca.get_port()); +TEST(CliArg, UdpLegacyOut) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://127.0.0.1:555")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "127.0.0.1"); + EXPECT_EQ(udp->port, 555); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::Out); +} + +TEST(CliArg, UdpLegacyIn) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://:777")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(udp->port, 777); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::In); +} + +TEST(CliArg, UdpLegacyInAllInterfaces) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://0.0.0.0:888")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(udp->port, 888); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::In); +} - ca.parse("udp://0.0.0.0"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "0.0.0.0"); - EXPECT_EQ(0, ca.get_port()); +TEST(CliArg, UdpIp) +{ + CliArg ca; - ca.parse("udp://0.0.0.0:7"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "0.0.0.0"); - EXPECT_EQ(7, ca.get_port()); + EXPECT_TRUE(ca.parse("udp://0.0.0.0:7")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(7, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::In); +} - ca.parse("udp://localhost:99"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "localhost"); - EXPECT_EQ(99, ca.get_port()); +TEST(CliArg, UdpLocalhost) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://localhost:99")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "localhost"); + EXPECT_EQ(99, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::Out); +} - ca.parse("udp://example.com"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "example.com"); - EXPECT_EQ(0, ca.get_port()); +TEST(CliArg, UdpDomain) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://example.com:77")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "example.com"); + EXPECT_EQ(77, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::Out); +} - ca.parse("udp://something.local:42"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Udp); - EXPECT_STREQ(ca.get_path().c_str(), "something.local"); - EXPECT_EQ(42, ca.get_port()); +TEST(CliArg, UdpLocalDomain) +{ + CliArg ca; + EXPECT_TRUE(ca.parse("udp://something.local:42")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "something.local"); + EXPECT_EQ(42, udp->port); +} +TEST(CliArg, UdpLegacyWrong) +{ + CliArg ca; // All the wrong combinations. EXPECT_FALSE(ca.parse("")); EXPECT_FALSE(ca.parse("udp:/localhost:99")); @@ -64,45 +116,138 @@ TEST(CliArg, UDPConnections) EXPECT_FALSE(ca.parse("udp://0.0.0.0:-5")); } -TEST(CliArg, TCPConnections) +TEST(CliArg, UdpInAll) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("udpin://0.0.0.0:55")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(55, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::In); +} + +TEST(CliArg, UdpInSpecific) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("udpin://192.168.0.5:66")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "192.168.0.5"); + EXPECT_EQ(66, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::In); +} + +TEST(CliArg, UdpOutInvalid) +{ + CliArg ca; + + EXPECT_FALSE(ca.parse("udpout://0.0.0.0:55")); +} + +TEST(CliArg, UdpOutpecific) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("udpout://192.168.0.5:66")); + auto udp = std::get_if(&ca.protocol); + ASSERT_TRUE(udp); + EXPECT_STREQ(udp->host.c_str(), "192.168.0.5"); + EXPECT_EQ(66, udp->port); + EXPECT_EQ(udp->mode, CliArg::Udp::Mode::Out); +} + +TEST(CliArg, TcpLegacyNoPort) +{ + CliArg ca; + + EXPECT_FALSE(ca.parse("tcp://127.0.0.1")); + EXPECT_TRUE(std::get_if(&ca.protocol)); +} + +TEST(CliArg, TcpLegacyNoHostNoPort) +{ + CliArg ca; + + EXPECT_FALSE(ca.parse("tcp://")); + EXPECT_TRUE(std::get_if(&ca.protocol)); +} + +TEST(CliArg, TcpLegacyInAllHost) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("tcp://:888")); + + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + + EXPECT_STREQ(tcp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(tcp->port, 888); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::In); +} + +TEST(CliArg, TcpLegacyOutIp) { CliArg ca; - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::None); - EXPECT_TRUE(ca.parse("tcp://127.0.0.1")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), "127.0.0.1"); - EXPECT_EQ(0, ca.get_port()); + EXPECT_TRUE(ca.parse("tcp://127.0.0.1:999")); - ca.parse("tcp://"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), ""); - EXPECT_EQ(0, ca.get_port()); + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); - ca.parse("tcp://:8"); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), ""); - EXPECT_EQ(8, ca.get_port()); + EXPECT_STREQ(tcp->host.c_str(), "127.0.0.1"); + EXPECT_EQ(tcp->port, 999); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::Out); +} - EXPECT_TRUE(ca.parse("tcp://127.0.0.1:7")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), "127.0.0.1"); - EXPECT_EQ(7, ca.get_port()); +TEST(CliArg, TcpLegacyOutLocalhost) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("tcp://localhost:444")); - EXPECT_TRUE(ca.parse("tcp://localhost:99")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), "localhost"); - EXPECT_EQ(99, ca.get_port()); + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + + EXPECT_STREQ(tcp->host.c_str(), "localhost"); + EXPECT_EQ(tcp->port, 444); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::Out); +} + +TEST(CliArg, TcpLegacyOutDomain) +{ + CliArg ca; EXPECT_TRUE(ca.parse("tcp://example.com:1234")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), "example.com"); - EXPECT_EQ(1234, ca.get_port()); + + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + + EXPECT_STREQ(tcp->host.c_str(), "example.com"); + EXPECT_EQ(tcp->port, 1234); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::Out); +} + +TEST(CliArg, TcpLegacyOutLocal) +{ + CliArg ca; EXPECT_TRUE(ca.parse("tcp://something.local:42")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Tcp); - EXPECT_STREQ(ca.get_path().c_str(), "something.local"); - EXPECT_EQ(42, ca.get_port()); + + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + + EXPECT_STREQ(tcp->host.c_str(), "something.local"); + EXPECT_EQ(tcp->port, 42); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::Out); +} + +TEST(CliArg, TcpLegacyWrong) +{ + CliArg ca; // All the wrong combinations. EXPECT_FALSE(ca.parse("")); @@ -116,54 +261,115 @@ TEST(CliArg, TCPConnections) EXPECT_FALSE(ca.parse("tcp://127.0.0.1:-5")); } -TEST(CliArg, SerialConnections) +TEST(CliArg, TcpInAll) { CliArg ca; - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::None); - EXPECT_TRUE(ca.parse("serial:///dev/ttyS0")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Serial); - EXPECT_STREQ(ca.get_path().c_str(), "/dev/ttyS0"); - EXPECT_EQ(0, ca.get_baudrate()); - EXPECT_EQ(false, ca.get_flow_control()); + EXPECT_TRUE(ca.parse("tcpin://0.0.0.0:55")); + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + EXPECT_STREQ(tcp->host.c_str(), "0.0.0.0"); + EXPECT_EQ(55, tcp->port); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::In); +} - EXPECT_TRUE(ca.parse("serial_flowcontrol:///dev/ttyS0:4000000")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Serial); - EXPECT_STREQ(ca.get_path().c_str(), "/dev/ttyS0"); - EXPECT_EQ(4000000, ca.get_baudrate()); - EXPECT_EQ(true, ca.get_flow_control()); +TEST(CliArg, TcpInSpecific) +{ + CliArg ca; - EXPECT_TRUE(ca.parse("serial://COM13:57600")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Serial); - EXPECT_STREQ(ca.get_path().c_str(), "COM13"); - EXPECT_EQ(57600, ca.get_baudrate()); - EXPECT_EQ(false, ca.get_flow_control()); + EXPECT_TRUE(ca.parse("tcpin://192.168.0.5:66")); + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + EXPECT_STREQ(tcp->host.c_str(), "192.168.0.5"); + EXPECT_EQ(66, tcp->port); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::In); +} + +TEST(CliArg, TcpOutInvalid) +{ + CliArg ca; + + EXPECT_FALSE(ca.parse("tcpout://0.0.0.0:55")); +} + +TEST(CliArg, TcpOutpecific) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("tcpout://192.168.0.5:66")); + auto tcp = std::get_if(&ca.protocol); + ASSERT_TRUE(tcp); + EXPECT_STREQ(tcp->host.c_str(), "192.168.0.5"); + EXPECT_EQ(66, tcp->port); + EXPECT_EQ(tcp->mode, CliArg::Tcp::Mode::Out); +} + +TEST(CliArg, SerialPort) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("serial:///dev/ttyS0:115200")); + auto serial = std::get_if(&ca.protocol); + ASSERT_TRUE(serial); + EXPECT_STREQ(serial->path.c_str(), "/dev/ttyS0"); + EXPECT_EQ(serial->baudrate, 115200); + EXPECT_EQ(serial->flow_control_enabled, false); +} + +TEST(CliArg, SerialName) +{ + CliArg ca; EXPECT_TRUE(ca.parse("serial:///dev/tty.usbmodem1:115200")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Serial); - EXPECT_STREQ(ca.get_path().c_str(), "/dev/tty.usbmodem1"); - EXPECT_EQ(115200, ca.get_baudrate()); - EXPECT_EQ(false, ca.get_flow_control()); + auto serial = std::get_if(&ca.protocol); + ASSERT_TRUE(serial); + EXPECT_STREQ(serial->path.c_str(), "/dev/tty.usbmodem1"); + EXPECT_EQ(serial->baudrate, 115200); + EXPECT_EQ(serial->flow_control_enabled, false); +} - EXPECT_TRUE(ca.parse("serial://COM3")); - EXPECT_EQ(ca.get_protocol(), CliArg::Protocol::Serial); - EXPECT_STREQ(ca.get_path().c_str(), "COM3"); - EXPECT_EQ(0, ca.get_baudrate()); - EXPECT_EQ(false, ca.get_flow_control()); +TEST(CliArg, SerialWithFlowControl) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("serial_flowcontrol:///dev/ttyS0:4000000")); + auto serial = std::get_if(&ca.protocol); + ASSERT_TRUE(serial); + EXPECT_STREQ(serial->path.c_str(), "/dev/ttyS0"); + EXPECT_EQ(serial->baudrate, 4000000); + EXPECT_EQ(serial->flow_control_enabled, true); +} + +TEST(CliArg, SerialWindowsComport) +{ + CliArg ca; + + EXPECT_TRUE(ca.parse("serial://COM13:57600")); + auto serial = std::get_if(&ca.protocol); + EXPECT_STREQ(serial->path.c_str(), "COM13"); + EXPECT_EQ(serial->baudrate, 57600); + EXPECT_EQ(serial->flow_control_enabled, false); +} + +TEST(CliArg, SerialWrong) +{ + CliArg ca; // All the wrong combinations. EXPECT_FALSE(ca.parse("")); - EXPECT_FALSE(ca.parse("serial///dev/ttyS0")); - EXPECT_FALSE(ca.parse("/serial:///dev/ttyS0")); - EXPECT_FALSE(ca.parse("serial:/dev/ttyS0")); - EXPECT_FALSE(ca.parse("serial://dev/ttyS0")); + EXPECT_FALSE(ca.parse("serial:///dev/ttyS0")); + EXPECT_FALSE(ca.parse("serial///dev/ttyS0:57600")); + EXPECT_FALSE(ca.parse("/serial:///dev/ttyS0:57600")); + EXPECT_FALSE(ca.parse("serial:/dev/ttyS0:57600")); + EXPECT_FALSE(ca.parse("serial://dev/ttyS0:57600")); EXPECT_FALSE(ca.parse("serial://")); - EXPECT_FALSE(ca.parse("serial://COM")); EXPECT_FALSE(ca.parse("serial://COM:57600")); - EXPECT_FALSE(ca.parse("seri://COM3")); + EXPECT_FALSE(ca.parse("serial://COM:57600")); + EXPECT_FALSE(ca.parse("seri://COM3:57600")); EXPECT_FALSE(ca.parse("seri://COM3:57600")); EXPECT_FALSE(ca.parse("serial://")); - EXPECT_FALSE(ca.parse("serial://SOM3")); + EXPECT_FALSE(ca.parse("serial://SOM3:57600")); EXPECT_FALSE(ca.parse("serial://SOM3:57600")); EXPECT_FALSE(ca.parse("serial://COM3:-1")); -} + EXPECT_FALSE(ca.parse("serial://COM3")); +} \ No newline at end of file diff --git a/src/mavsdk/core/include/mavsdk/mavsdk.h b/src/mavsdk/core/include/mavsdk/mavsdk.h index 59fdda88af..e3cfc25346 100644 --- a/src/mavsdk/core/include/mavsdk/mavsdk.h +++ b/src/mavsdk/core/include/mavsdk/mavsdk.h @@ -37,20 +37,6 @@ class MavsdkImpl; */ class Mavsdk { public: - /** @brief Default UDP bind IP (accepts any incoming connections). */ - static constexpr auto DEFAULT_UDP_BIND_IP = "0.0.0.0"; - /** @brief Default UDP bind port (same port as used by MAVROS). */ - static constexpr int DEFAULT_UDP_PORT = 14540; - /** @brief Default TCP remote IP (localhost). */ - static constexpr auto DEFAULT_TCP_REMOTE_IP = "127.0.0.1"; - /** @brief Default TCP remote port. */ - static constexpr int DEFAULT_TCP_REMOTE_PORT = 5760; - /** @brief Default serial baudrate. */ - static constexpr int DEFAULT_SERIAL_BAUDRATE = 57600; - - /** @brief Default internal timeout in seconds. */ - static constexpr double DEFAULT_TIMEOUT_S = 0.5; - /** * @brief Returns the version of MAVSDK. * @@ -65,14 +51,24 @@ class Mavsdk { * * Supports connection: Serial, TCP or UDP. * Connection URL format should be: - * - UDP: udp://[host][:bind_port] - * - TCP: tcp://[host][:remote_port] - * - Serial: serial://dev_node[:baudrate] * - * For UDP, the host can be set to either: - * - zero IP: 0.0.0.0 -> behave like a server and listen for heartbeats. - * - some IP: 192.168.1.12 -> behave like a client, initiate connection - * and start sending heartbeats. + * - UDP in (server): udpin://our_ip:port + * - UDP out (client): udpout://remote_ip:port + * + * - TCP in (server): tcpin://our_ip:port + * - TCP out (client): tcpout://remote_ip:port + * + * - Serial: serial://dev_node:baudrate + * - Serial with flow control: serial_flowcontrol://dev_node:baudrate + * + * For UDP in and TCP in (as server), our IP can be set to: + * - 0.0.0.0: listen on all interfaces + * - 127.0.0.1: listen on loopback (local) interface only + * - Our IP: (e.g. 192.168.1.12): listen only on the network interface + * with this IP. + * + * For UDP out and TCP out, the IP needs to be set to the remote IP, + * where the MAVLink messages are to be sent to. * * @param connection_url connection URL string. * @param forwarding_option message forwarding option (when multiple interfaces are used). @@ -93,14 +89,24 @@ class Mavsdk { * * Supports connection: Serial, TCP or UDP. * Connection URL format should be: - * - UDP: udp://[host][:bind_port] - * - TCP: tcp://[host][:remote_port] - * - Serial: serial://dev_node[:baudrate] * - * For UDP, the host can be set to either: - * - zero IP: 0.0.0.0 -> behave like a server and listen for heartbeats. - * - some IP: 192.168.1.12 -> behave like a client, initiate connection - * and start sending heartbeats. + * - UDP in (server): udpin://our_ip:port + * - UDP out (client): udpout://remote_ip:port + * + * - TCP in (server): tcpin://our_ip:port + * - TCP out (client): tcpout://remote_ip:port + * + * - Serial: serial://dev_node:baudrate + * - Serial with flow control: serial_flowcontrol://dev_node:baudrate + * + * For UDP in and TCP in (as server), our IP can be set to: + * - 0.0.0.0: listen on all interfaces + * - 127.0.0.1: listen on loopback (local) interface only + * - Our IP: (e.g. 192.168.1.12): listen only on the network interface + * with this IP. + * + * For UDP out and TCP out, the IP needs to be set to the remote IP, + * where the MAVLink messages are to be sent to. * * @param connection_url connection URL string. * @param forwarding_option message forwarding option (when multiple interfaces are used). @@ -111,78 +117,6 @@ class Mavsdk { const std::string& connection_url, ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - /** - * @brief Adds a UDP connection to the specified port number. - * - * Any incoming connections are accepted (0.0.0.0). - * - * @param local_port The local UDP port to listen to (defaults to 14540, the same as MAVROS). - * @param forwarding_option message forwarding option (when multiple interfaces are used). - * @return The result of adding the connection. - */ - ConnectionResult add_udp_connection( - int local_port = DEFAULT_UDP_PORT, - ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - - /** - * @brief Adds a UDP connection to the specified port number and local interface. - * - * To accept only local connections of the machine, use 127.0.0.1. - * For any incoming connections, use 0.0.0.0. - * - * @param local_ip The local UDP IP address to listen to. - * @param local_port The local UDP port to listen to (defaults to 14540, the same as MAVROS). - * @param forwarding_option message forwarding option (when multiple interfaces are used). - * @return The result of adding the connection. - */ - ConnectionResult add_udp_connection( - const std::string& local_ip, - int local_port = DEFAULT_UDP_PORT, - ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - - /** - * @brief Sets up instance to send heartbeats to the specified remote interface and port number. - * - * @param remote_ip The remote UDP IP address to report to. - * @param remote_port The local UDP port to report to. - * @param forwarding_option message forwarding option (when multiple interfaces are used). - * @return The result of operation. - */ - ConnectionResult setup_udp_remote( - const std::string& remote_ip, - int remote_port, - ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - - /** - * @brief Adds a TCP connection with a specific IP address and port number. - * - * @param remote_ip Remote IP address to connect to. - * @param remote_port The TCP port to connect to (defaults to 5760). - * @param forwarding_option message forwarding option (when multiple interfaces are used). - * @return The result of adding the connection. - */ - ConnectionResult add_tcp_connection( - const std::string& remote_ip, - int remote_port = DEFAULT_TCP_REMOTE_PORT, - ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - - /** - * @brief Adds a serial connection with a specific port (COM or UART dev node) and baudrate as - * specified. - * - * - * @param dev_path COM or UART dev node name/path (e.g. "/dev/ttyS0", or "COM3" on Windows). - * @param baudrate Baudrate of the serial port (defaults to 57600). - * @param flow_control enable/disable flow control. - * @param forwarding_option message forwarding option (when multiple interfaces are used). - * @return The result of adding the connection. - */ - ConnectionResult add_serial_connection( - const std::string& dev_path, - int baudrate = DEFAULT_SERIAL_BAUDRATE, - bool flow_control = false, - ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - /** * Remove connection again. * @@ -434,6 +368,15 @@ class Mavsdk { void intercept_outgoing_messages_async(std::function callback); private: + static constexpr int DEFAULT_SYSTEM_ID_GCS = 245; + static constexpr int DEFAULT_COMPONENT_ID_GCS = MAV_COMP_ID_MISSIONPLANNER; + static constexpr int DEFAULT_SYSTEM_ID_CC = 1; + static constexpr int DEFAULT_COMPONENT_ID_CC = MAV_COMP_ID_PATHPLANNER; + static constexpr int DEFAULT_SYSTEM_ID_AUTOPILOT = 1; + static constexpr int DEFAULT_COMPONENT_ID_AUTOPILOT = MAV_COMP_ID_AUTOPILOT1; + static constexpr int DEFAULT_SYSTEM_ID_CAMERA = 1; + static constexpr int DEFAULT_COMPONENT_ID_CAMERA = MAV_COMP_ID_CAMERA; + /* @private. */ std::shared_ptr _impl{}; diff --git a/src/mavsdk/core/mavsdk.cpp b/src/mavsdk/core/mavsdk.cpp index b871bd7bef..3e2314121b 100644 --- a/src/mavsdk/core/mavsdk.cpp +++ b/src/mavsdk/core/mavsdk.cpp @@ -28,38 +28,6 @@ std::pair Mavsdk::add_any_connection return _impl->add_any_connection(connection_url, forwarding_option); } -ConnectionResult Mavsdk::add_udp_connection(int local_port, ForwardingOption forwarding_option) -{ - return _impl->add_udp_connection(DEFAULT_UDP_BIND_IP, local_port, forwarding_option).first; -} - -ConnectionResult Mavsdk::add_udp_connection( - const std::string& local_bind_ip, const int local_port, ForwardingOption forwarding_option) -{ - return _impl->add_udp_connection(local_bind_ip, local_port, forwarding_option).first; -} - -ConnectionResult Mavsdk::setup_udp_remote( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option) -{ - return _impl->setup_udp_remote(remote_ip, remote_port, forwarding_option).first; -} - -ConnectionResult Mavsdk::add_tcp_connection( - const std::string& remote_ip, const int remote_port, ForwardingOption forwarding_option) -{ - return _impl->add_tcp_connection(remote_ip, remote_port, forwarding_option).first; -} - -ConnectionResult Mavsdk::add_serial_connection( - const std::string& dev_path, - const int baudrate, - bool flow_control, - ForwardingOption forwarding_option) -{ - return _impl->add_serial_connection(dev_path, baudrate, flow_control, forwarding_option).first; -} - void Mavsdk::remove_connection(ConnectionHandle handle) { _impl->remove_connection(handle); @@ -122,13 +90,13 @@ Mavsdk::Configuration::Configuration( Mavsdk::ComponentType Mavsdk::Configuration::component_type_for_component_id(uint8_t component_id) { switch (component_id) { - case MavsdkImpl::DEFAULT_COMPONENT_ID_GCS: + case Mavsdk::DEFAULT_COMPONENT_ID_GCS: return ComponentType::GroundStation; - case MavsdkImpl::DEFAULT_COMPONENT_ID_CC: + case Mavsdk::DEFAULT_COMPONENT_ID_CC: return ComponentType::CompanionComputer; - case MavsdkImpl::DEFAULT_COMPONENT_ID_AUTOPILOT: + case Mavsdk::DEFAULT_COMPONENT_ID_AUTOPILOT: return ComponentType::Autopilot; - case MavsdkImpl::DEFAULT_COMPONENT_ID_CAMERA: + case Mavsdk::DEFAULT_COMPONENT_ID_CAMERA: return ComponentType::Camera; default: return ComponentType::Custom; @@ -136,31 +104,31 @@ Mavsdk::ComponentType Mavsdk::Configuration::component_type_for_component_id(uin } Mavsdk::Configuration::Configuration(ComponentType component_type) : - _system_id(MavsdkImpl::DEFAULT_SYSTEM_ID_GCS), - _component_id(MavsdkImpl::DEFAULT_COMPONENT_ID_GCS), + _system_id(Mavsdk::DEFAULT_SYSTEM_ID_GCS), + _component_id(Mavsdk::DEFAULT_COMPONENT_ID_GCS), _always_send_heartbeats(false), _component_type(component_type) { switch (component_type) { case Mavsdk::ComponentType::GroundStation: - _system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_GCS; - _component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_GCS; + _system_id = Mavsdk::DEFAULT_SYSTEM_ID_GCS; + _component_id = Mavsdk::DEFAULT_COMPONENT_ID_GCS; _always_send_heartbeats = false; break; case Mavsdk::ComponentType::CompanionComputer: // TODO implement auto-detection of system ID - maybe from heartbeats? - _system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_CC; - _component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_CC; + _system_id = Mavsdk::DEFAULT_SYSTEM_ID_CC; + _component_id = Mavsdk::DEFAULT_COMPONENT_ID_CC; _always_send_heartbeats = true; break; case Mavsdk::ComponentType::Autopilot: - _system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_AUTOPILOT; - _component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_AUTOPILOT; + _system_id = Mavsdk::DEFAULT_SYSTEM_ID_AUTOPILOT; + _component_id = Mavsdk::DEFAULT_COMPONENT_ID_AUTOPILOT; _always_send_heartbeats = true; break; case Mavsdk::ComponentType::Camera: - _system_id = MavsdkImpl::DEFAULT_SYSTEM_ID_CAMERA; - _component_id = MavsdkImpl::DEFAULT_COMPONENT_ID_CAMERA; + _system_id = Mavsdk::DEFAULT_SYSTEM_ID_CAMERA; + _component_id = Mavsdk::DEFAULT_COMPONENT_ID_CAMERA; _always_send_heartbeats = true; break; default: diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index 84977229d5..489542b44f 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -12,6 +12,7 @@ #include "cli_arg.h" #include "version.h" #include "server_component_impl.h" +#include "plugin_base.h" #include "mavlink_channels.h" #include "callback_list.tpp" @@ -467,44 +468,33 @@ std::pair MavsdkImpl::add_any_connec return {ConnectionResult::ConnectionUrlInvalid, Mavsdk::ConnectionHandle{}}; } - switch (cli_arg.get_protocol()) { - case CliArg::Protocol::Udp: { - 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); - } - } - - case CliArg::Protocol::Tcp: { - std::string path = Mavsdk::DEFAULT_TCP_REMOTE_IP; - int port = Mavsdk::DEFAULT_TCP_REMOTE_PORT; - if (!cli_arg.get_path().empty()) { - path = cli_arg.get_path(); - } - if (cli_arg.get_port()) { - port = cli_arg.get_port(); - } - return add_tcp_connection(path, port, forwarding_option); - } - - case CliArg::Protocol::Serial: { - int baudrate = Mavsdk::DEFAULT_SERIAL_BAUDRATE; - if (cli_arg.get_baudrate()) { - baudrate = cli_arg.get_baudrate(); - } - bool flow_control = cli_arg.get_flow_control(); - return add_serial_connection( - cli_arg.get_path(), baudrate, flow_control, forwarding_option); - } - - default: - return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; - } + return std::visit( + overloaded{ + [&](std::monostate) -> std::pair { + // Should not happen anyway. + return {ConnectionResult::ConnectionUrlInvalid, Mavsdk::ConnectionHandle()}; + }, + [&](CliArg::Udp& udp) -> std::pair { + if (udp.mode == CliArg::Udp::Mode::In) { + return add_udp_connection(udp.host, udp.port, forwarding_option); + } else { + return setup_udp_remote(udp.host, udp.port, forwarding_option); + } + }, + [&](CliArg::Tcp& tcp) -> std::pair { + if (tcp.mode == CliArg::Tcp::Mode::In) { + // TODO: implement + return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle()}; + } else { + return add_tcp_connection(tcp.host, tcp.port, forwarding_option); + } + // TODO in out + }, + [&](CliArg::Serial& serial) -> std::pair { + return add_serial_connection( + serial.path, serial.baudrate, serial.flow_control_enabled, forwarding_option); + }}, + cli_arg.protocol); } std::pair MavsdkImpl::add_udp_connection( diff --git a/src/mavsdk/core/mavsdk_impl.h b/src/mavsdk/core/mavsdk_impl.h index 40e2f49e04..830d088d30 100644 --- a/src/mavsdk/core/mavsdk_impl.h +++ b/src/mavsdk/core/mavsdk_impl.h @@ -28,23 +28,6 @@ namespace mavsdk { class MavsdkImpl { public: - /** @brief Default System ID for GCS configuration type. */ - static constexpr int DEFAULT_SYSTEM_ID_GCS = 245; - /** @brief Default Component ID for GCS configuration type. */ - static constexpr int DEFAULT_COMPONENT_ID_GCS = MAV_COMP_ID_MISSIONPLANNER; - /** @brief Default System ID for CompanionComputer configuration type. */ - static constexpr int DEFAULT_SYSTEM_ID_CC = 1; - /** @brief Default Component ID for CompanionComputer configuration type. */ - static constexpr int DEFAULT_COMPONENT_ID_CC = MAV_COMP_ID_PATHPLANNER; - /** @brief Default System ID for Autopilot configuration type. */ - static constexpr int DEFAULT_SYSTEM_ID_AUTOPILOT = 1; - /** @brief Default Component ID for Autopilot configuration type. */ - static constexpr int DEFAULT_COMPONENT_ID_AUTOPILOT = MAV_COMP_ID_AUTOPILOT1; - /** @brief Default System ID for Camera configuration type. */ - static constexpr int DEFAULT_SYSTEM_ID_CAMERA = 1; - /** @brief Default Component ID for Camera configuration type. */ - static constexpr int DEFAULT_COMPONENT_ID_CAMERA = MAV_COMP_ID_CAMERA; - MavsdkImpl(const Mavsdk::Configuration& configuration); ~MavsdkImpl(); MavsdkImpl(const MavsdkImpl&) = delete; @@ -57,18 +40,6 @@ class MavsdkImpl { std::pair add_any_connection(const std::string& connection_url, ForwardingOption forwarding_option); - std::pair add_udp_connection( - const std::string& local_ip, int local_port_number, ForwardingOption forwarding_option); - std::pair add_tcp_connection( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); - std::pair add_serial_connection( - const std::string& dev_path, - int baudrate, - bool flow_control, - ForwardingOption forwarding_option); - std::pair setup_udp_remote( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); - void remove_connection(Mavsdk::ConnectionHandle handle); std::vector> systems() const; @@ -122,6 +93,20 @@ class MavsdkImpl { ServerComponentImpl& default_server_component_impl(); private: + static constexpr float DEFAULT_TIMEOUT_S = 0.5f; + + std::pair add_udp_connection( + const std::string& local_ip, int local_port_number, ForwardingOption forwarding_option); + std::pair add_tcp_connection( + const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); + std::pair add_serial_connection( + const std::string& dev_path, + int baudrate, + bool flow_control, + ForwardingOption forwarding_option); + std::pair setup_udp_remote( + const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); + Mavsdk::ConnectionHandle add_connection(const std::shared_ptr&); void make_system_with_component(uint8_t system_id, uint8_t component_id); @@ -178,7 +163,7 @@ class MavsdkImpl { std::function _intercept_incoming_messages_callback{nullptr}; std::function _intercept_outgoing_messages_callback{nullptr}; - std::atomic _timeout_s{Mavsdk::DEFAULT_TIMEOUT_S}; + std::atomic _timeout_s{}; static constexpr double HEARTBEAT_SEND_INTERVAL_S = 1.0; CallEveryHandler::Cookie _heartbeat_send_cookie{}; From 00ed1a29b55fa5081abe6f85618227051e1a2430 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Jun 2024 20:22:15 +0200 Subject: [PATCH 02/17] core: combine add_udp_conection, setup_udp_remote A bit less duplication like this. --- src/mavsdk/core/mavsdk_impl.cpp | 49 ++++++++++----------------------- src/mavsdk/core/mavsdk_impl.h | 9 +++--- 2 files changed, 19 insertions(+), 39 deletions(-) diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index 489542b44f..b1e3d69ce6 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -9,7 +9,6 @@ #include "system.h" #include "system_impl.h" #include "serial_connection.h" -#include "cli_arg.h" #include "version.h" #include "server_component_impl.h" #include "plugin_base.h" @@ -475,11 +474,7 @@ std::pair MavsdkImpl::add_any_connec return {ConnectionResult::ConnectionUrlInvalid, Mavsdk::ConnectionHandle()}; }, [&](CliArg::Udp& udp) -> std::pair { - if (udp.mode == CliArg::Udp::Mode::In) { - return add_udp_connection(udp.host, udp.port, forwarding_option); - } else { - return setup_udp_remote(udp.host, udp.port, forwarding_option); - } + return add_udp_connection(udp, forwarding_option); }, [&](CliArg::Tcp& tcp) -> std::pair { if (tcp.mode == CliArg::Tcp::Mode::In) { @@ -497,55 +492,39 @@ std::pair MavsdkImpl::add_any_connec cli_arg.protocol); } -std::pair MavsdkImpl::add_udp_connection( - const std::string& local_ip, const int local_port, ForwardingOption forwarding_option) +std::pair +MavsdkImpl::add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwarding_option) { auto new_conn = std::make_shared( [this](mavlink_message_t& message, Connection* connection) { receive_message(message, connection); }, - local_ip, - local_port, + udp.mode == CliArg::Udp::Mode::In ? udp.host : "0.0.0.0", + udp.mode == CliArg::Udp::Mode::In ? udp.port : 0, forwarding_option); + if (!new_conn) { return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; } + ConnectionResult ret = new_conn->start(); - if (ret == ConnectionResult::Success) { - return {ret, add_connection(new_conn)}; - } else { + + if (ret != ConnectionResult::Success) { return {ret, Mavsdk::ConnectionHandle{}}; } -} -std::pair MavsdkImpl::setup_udp_remote( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option) -{ - auto new_conn = std::make_shared( - [this](mavlink_message_t& message, Connection* connection) { - receive_message(message, connection); - }, - "0.0.0.0", - 0, - forwarding_option); - if (!new_conn) { - return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; - } - ConnectionResult ret = new_conn->start(); - if (ret == ConnectionResult::Success) { - new_conn->add_remote(remote_ip, remote_port); - auto handle = add_connection(new_conn); + auto handle = add_connection(new_conn); + + if (udp.mode == CliArg::Udp::Mode::Out) { + new_conn->add_remote(udp.host, udp.port); std::lock_guard lock(_systems_mutex); // With a UDP remote, we need to initiate the connection by sending heartbeats. auto new_configuration = get_configuration(); new_configuration.set_always_send_heartbeats(true); set_configuration(new_configuration); - - return {ret, handle}; - } else { - return {ret, Mavsdk::ConnectionHandle{}}; } + return {ConnectionResult::Success, handle}; } std::pair MavsdkImpl::add_tcp_connection( diff --git a/src/mavsdk/core/mavsdk_impl.h b/src/mavsdk/core/mavsdk_impl.h index 830d088d30..5bb2f0ca32 100644 --- a/src/mavsdk/core/mavsdk_impl.h +++ b/src/mavsdk/core/mavsdk_impl.h @@ -11,7 +11,9 @@ #include "autopilot.h" #include "call_every_handler.h" #include "connection.h" +#include "cli_arg.h" #include "handle_factory.h" +#include "handle.h" #include "mavsdk.h" #include "mavlink_include.h" #include "mavlink_address.h" @@ -95,8 +97,9 @@ class MavsdkImpl { private: static constexpr float DEFAULT_TIMEOUT_S = 0.5f; - std::pair add_udp_connection( - const std::string& local_ip, int local_port_number, ForwardingOption forwarding_option); + std::pair + add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwarding_option); + std::pair add_tcp_connection( const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); std::pair add_serial_connection( @@ -104,8 +107,6 @@ class MavsdkImpl { int baudrate, bool flow_control, ForwardingOption forwarding_option); - std::pair setup_udp_remote( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); Mavsdk::ConnectionHandle add_connection(const std::shared_ptr&); void make_system_with_component(uint8_t system_id, uint8_t component_id); From ed25c5eb5e3948dc7ecb1c4b295f70a117422b15 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Jun 2024 20:30:45 +0200 Subject: [PATCH 03/17] core: TcpConnection -> TcpClientConnection Rename the file and class, so that we can add a TCP server connection as well. --- src/mavsdk/core/CMakeLists.txt | 2 +- src/mavsdk/core/mavsdk_impl.cpp | 4 ++-- ...nnection.cpp => tcp_client_connection.cpp} | 20 +++++++++---------- ...p_connection.h => tcp_client_connection.h} | 10 +++++----- 4 files changed, 18 insertions(+), 18 deletions(-) rename src/mavsdk/core/{tcp_connection.cpp => tcp_client_connection.cpp} (91%) rename src/mavsdk/core/{tcp_connection.h => tcp_client_connection.h} (79%) diff --git a/src/mavsdk/core/CMakeLists.txt b/src/mavsdk/core/CMakeLists.txt index 8b2a7729cc..700a7c258e 100644 --- a/src/mavsdk/core/CMakeLists.txt +++ b/src/mavsdk/core/CMakeLists.txt @@ -52,7 +52,7 @@ target_sources(mavsdk server_component.cpp server_component_impl.cpp server_plugin_impl_base.cpp - tcp_connection.cpp + tcp_client_connection.cpp timeout_handler.cpp udp_connection.cpp log.cpp diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index b1e3d69ce6..d7ba5bff58 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -4,7 +4,7 @@ #include #include "connection.h" -#include "tcp_connection.h" +#include "tcp_client_connection.h" #include "udp_connection.h" #include "system.h" #include "system_impl.h" @@ -530,7 +530,7 @@ MavsdkImpl::add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwardi std::pair MavsdkImpl::add_tcp_connection( const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option) { - auto new_conn = std::make_shared( + auto new_conn = std::make_shared( [this](mavlink_message_t& message, Connection* connection) { receive_message(message, connection); }, diff --git a/src/mavsdk/core/tcp_connection.cpp b/src/mavsdk/core/tcp_client_connection.cpp similarity index 91% rename from src/mavsdk/core/tcp_connection.cpp rename to src/mavsdk/core/tcp_client_connection.cpp index d645b31ef6..54829ca8c3 100644 --- a/src/mavsdk/core/tcp_connection.cpp +++ b/src/mavsdk/core/tcp_client_connection.cpp @@ -1,4 +1,4 @@ -#include "tcp_connection.h" +#include "tcp_client_connection.h" #include "log.h" #ifdef WINDOWS @@ -26,7 +26,7 @@ namespace mavsdk { /* change to remote_ip and remote_port */ -TcpConnection::TcpConnection( +TcpClientConnection::TcpClientConnection( Connection::ReceiverCallback receiver_callback, std::string remote_ip, int remote_port, @@ -37,13 +37,13 @@ TcpConnection::TcpConnection( _should_exit(false) {} -TcpConnection::~TcpConnection() +TcpClientConnection::~TcpClientConnection() { // If no one explicitly called stop before, we should at least do it. stop(); } -ConnectionResult TcpConnection::start() +ConnectionResult TcpClientConnection::start() { if (!start_mavlink_receiver()) { return ConnectionResult::ConnectionsExhausted; @@ -59,7 +59,7 @@ ConnectionResult TcpConnection::start() return ConnectionResult::Success; } -ConnectionResult TcpConnection::setup_port() +ConnectionResult TcpClientConnection::setup_port() { #ifdef WINDOWS WSADATA wsa; @@ -103,12 +103,12 @@ ConnectionResult TcpConnection::setup_port() return ConnectionResult::Success; } -void TcpConnection::start_recv_thread() +void TcpClientConnection::start_recv_thread() { - _recv_thread = std::make_unique(&TcpConnection::receive, this); + _recv_thread = std::make_unique(&TcpClientConnection::receive, this); } -ConnectionResult TcpConnection::stop() +ConnectionResult TcpClientConnection::stop() { _should_exit = true; @@ -138,7 +138,7 @@ ConnectionResult TcpConnection::stop() return ConnectionResult::Success; } -bool TcpConnection::send_message(const mavlink_message_t& message) +bool TcpClientConnection::send_message(const mavlink_message_t& message) { if (!_is_ok) { return false; @@ -189,7 +189,7 @@ bool TcpConnection::send_message(const mavlink_message_t& message) return true; } -void TcpConnection::receive() +void TcpClientConnection::receive() { // Enough for MTU 1500 bytes. char buffer[2048]; diff --git a/src/mavsdk/core/tcp_connection.h b/src/mavsdk/core/tcp_client_connection.h similarity index 79% rename from src/mavsdk/core/tcp_connection.h rename to src/mavsdk/core/tcp_client_connection.h index 785905f8a0..b243cb72b7 100644 --- a/src/mavsdk/core/tcp_connection.h +++ b/src/mavsdk/core/tcp_client_connection.h @@ -17,22 +17,22 @@ namespace mavsdk { -class TcpConnection : public Connection { +class TcpClientConnection : public Connection { public: - explicit TcpConnection( + explicit TcpClientConnection( Connection::ReceiverCallback receiver_callback, std::string remote_ip, int remote_port, ForwardingOption forwarding_option = ForwardingOption::ForwardingOff); - ~TcpConnection() override; + ~TcpClientConnection() override; ConnectionResult start() override; ConnectionResult stop() override; bool send_message(const mavlink_message_t& message) override; // Non-copyable - TcpConnection(const TcpConnection&) = delete; - const TcpConnection& operator=(const TcpConnection&) = delete; + TcpClientConnection(const TcpClientConnection&) = delete; + const TcpClientConnection& operator=(const TcpClientConnection&) = delete; private: ConnectionResult setup_port(); From a2b49b1d7f7feb180d711f55521586bb3a1fd6ee Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Jun 2024 21:26:10 +0200 Subject: [PATCH 04/17] core: add TCP server connection --- src/mavsdk/core/CMakeLists.txt | 3 +- src/mavsdk/core/mavsdk_impl.cpp | 61 +++--- src/mavsdk/core/mavsdk_impl.h | 4 +- src/mavsdk/core/tcp_client_connection.cpp | 1 + src/mavsdk/core/tcp_server_connection.cpp | 224 ++++++++++++++++++++++ src/mavsdk/core/tcp_server_connection.h | 39 ++++ 6 files changed, 306 insertions(+), 26 deletions(-) create mode 100644 src/mavsdk/core/tcp_server_connection.cpp create mode 100644 src/mavsdk/core/tcp_server_connection.h diff --git a/src/mavsdk/core/CMakeLists.txt b/src/mavsdk/core/CMakeLists.txt index 700a7c258e..28610f0be9 100644 --- a/src/mavsdk/core/CMakeLists.txt +++ b/src/mavsdk/core/CMakeLists.txt @@ -52,7 +52,8 @@ target_sources(mavsdk server_component.cpp server_component_impl.cpp server_plugin_impl_base.cpp - tcp_client_connection.cpp + tcp_client_connection.cpp + tcp_server_connection.cpp timeout_handler.cpp udp_connection.cpp log.cpp diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index d7ba5bff58..baa6eb8a87 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -2,9 +2,11 @@ #include #include +#include #include "connection.h" #include "tcp_client_connection.h" +#include "tcp_server_connection.h" #include "udp_connection.h" #include "system.h" #include "system_impl.h" @@ -477,13 +479,7 @@ std::pair MavsdkImpl::add_any_connec return add_udp_connection(udp, forwarding_option); }, [&](CliArg::Tcp& tcp) -> std::pair { - if (tcp.mode == CliArg::Tcp::Mode::In) { - // TODO: implement - return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle()}; - } else { - return add_tcp_connection(tcp.host, tcp.port, forwarding_option); - } - // TODO in out + return add_tcp_connection(tcp, forwarding_option); }, [&](CliArg::Serial& serial) -> std::pair { return add_serial_connection( @@ -527,24 +523,43 @@ MavsdkImpl::add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwardi return {ConnectionResult::Success, handle}; } -std::pair MavsdkImpl::add_tcp_connection( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option) +std::pair +MavsdkImpl::add_tcp_connection(const CliArg::Tcp& tcp, ForwardingOption forwarding_option) { - auto new_conn = std::make_shared( - [this](mavlink_message_t& message, Connection* connection) { - receive_message(message, connection); - }, - remote_ip, - remote_port, - forwarding_option); - if (!new_conn) { - return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; - } - ConnectionResult ret = new_conn->start(); - if (ret == ConnectionResult::Success) { - return {ret, add_connection(new_conn)}; + if (tcp.mode == CliArg::Tcp::Mode::Out) { + auto new_conn = std::make_shared( + [this](mavlink_message_t& message, Connection* connection) { + receive_message(message, connection); + }, + tcp.host, + tcp.port, + forwarding_option); + if (!new_conn) { + return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; + } + ConnectionResult ret = new_conn->start(); + if (ret == ConnectionResult::Success) { + return {ret, add_connection(new_conn)}; + } else { + return {ret, Mavsdk::ConnectionHandle{}}; + } } else { - return {ret, Mavsdk::ConnectionHandle{}}; + auto new_conn = std::make_shared( + [this](mavlink_message_t& message, Connection* connection) { + receive_message(message, connection); + }, + tcp.host, + tcp.port, + forwarding_option); + if (!new_conn) { + return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}}; + } + ConnectionResult ret = new_conn->start(); + if (ret == ConnectionResult::Success) { + return {ret, add_connection(new_conn)}; + } else { + return {ret, Mavsdk::ConnectionHandle{}}; + } } } diff --git a/src/mavsdk/core/mavsdk_impl.h b/src/mavsdk/core/mavsdk_impl.h index 5bb2f0ca32..319977df7c 100644 --- a/src/mavsdk/core/mavsdk_impl.h +++ b/src/mavsdk/core/mavsdk_impl.h @@ -100,8 +100,8 @@ class MavsdkImpl { std::pair add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwarding_option); - std::pair add_tcp_connection( - const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option); + std::pair + add_tcp_connection(const CliArg::Tcp& tcp, ForwardingOption forwarding_option); std::pair add_serial_connection( const std::string& dev_path, int baudrate, diff --git a/src/mavsdk/core/tcp_client_connection.cpp b/src/mavsdk/core/tcp_client_connection.cpp index 54829ca8c3..40364ecba8 100644 --- a/src/mavsdk/core/tcp_client_connection.cpp +++ b/src/mavsdk/core/tcp_client_connection.cpp @@ -16,6 +16,7 @@ #include #include +#include #ifndef WINDOWS #define GET_ERROR(_x) strerror(_x) diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp new file mode 100644 index 0000000000..8b7d74b71a --- /dev/null +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -0,0 +1,224 @@ +#include "tcp_server_connection.h" +#include "log.h" + +#include +#include +#include + +#ifdef WINDOWS +#ifndef MINGW +#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library. +#endif +#else +#include +#include +#include +#include +#include +#include // for close() +#endif + +#ifndef WINDOWS +#define GET_ERROR(_x) strerror(_x) +#else +#define GET_ERROR(_x) WSAGetLastError() +#endif + +namespace mavsdk { +TcpServerConnection::TcpServerConnection( + Connection::ReceiverCallback receiver_callback, + std::string local_ip, + int local_port, + ForwardingOption forwarding_option) : + Connection(std::move(receiver_callback), forwarding_option), + _local_ip(local_ip), + _local_port(local_port), + _server_socket_fd(-1), + _client_socket_fd(-1), + _should_exit(false), + _is_ok(false) +{} + +TcpServerConnection::~TcpServerConnection() +{ + stop(); +} + +ConnectionResult TcpServerConnection::start() +{ + if (!start_mavlink_receiver()) { + return ConnectionResult::ConnectionsExhausted; + } + +#ifdef WINDOWS + WSADATA wsa; + if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) { + LogErr() << "Error: Winsock failed, error: " << WSAGetLastError(); + return ConnectionResult::SocketError; + } +#endif + + _server_socket_fd = socket(AF_INET, SOCK_STREAM, 0); + if (_server_socket_fd < 0) { + LogErr() << "socket error: " << GET_ERROR(errno); + return ConnectionResult::SocketError; + } + + struct sockaddr_in server_addr {}; + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = INADDR_ANY; + server_addr.sin_port = htons(_local_port); + + if (bind(_server_socket_fd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { + LogErr() << "bind error: " << GET_ERROR(errno); + return ConnectionResult::SocketError; + } + + if (listen(_server_socket_fd, 3) < 0) { + LogErr() << "listen error: " << GET_ERROR(errno); + return ConnectionResult::SocketError; + } + + _is_ok = true; + _accept_thread = std::make_unique(&TcpServerConnection::accept_client, this); + + return ConnectionResult::Success; +} + +ConnectionResult TcpServerConnection::stop() +{ + _should_exit = true; + +#ifndef WINDOWS + shutdown(_client_socket_fd, SHUT_RDWR); + close(_client_socket_fd); + close(_server_socket_fd); +#else + shutdown(_client_socket_fd, SD_BOTH); + closesocket(_client_socket_fd); + closesocket(_server_socket_fd); + WSACleanup(); +#endif + + if (_accept_thread && _accept_thread->joinable()) { + _accept_thread->join(); + } + + if (_recv_thread && _recv_thread->joinable()) { + _recv_thread->join(); + } + + // We need to stop this after stopping the receive thread, otherwise + // it can happen that we interfere with the parsing of a message. + stop_mavlink_receiver(); + + return ConnectionResult::Success; +} + +bool TcpServerConnection::send_message(const mavlink_message_t& message) +{ + if (!_is_ok) { + return false; + } + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); + + assert(buffer_len <= MAVLINK_MAX_PACKET_LEN); + +#if !defined(MSG_NOSIGNAL) + auto flags = 0; +#else + auto flags = MSG_NOSIGNAL; +#endif + + const auto send_len = send(_client_socket_fd, buffer, buffer_len, flags); + + if (send_len != buffer_len) { + LogErr() << "send failure: " << GET_ERROR(errno); + return false; + } + return true; +} + +void TcpServerConnection::accept_client() +{ + fd_set readfds; + struct timeval timeout; + + // Set server socket to non-blocking + int flags = fcntl(_server_socket_fd, F_GETFL, 0); + fcntl(_server_socket_fd, F_SETFL, flags | O_NONBLOCK); + + while (!_should_exit) { + FD_ZERO(&readfds); + FD_SET(_server_socket_fd, &readfds); + + // Set timeout to 1 second + timeout.tv_sec = 1; + timeout.tv_usec = 0; + + int activity = select(_server_socket_fd + 1, &readfds, nullptr, nullptr, &timeout); + + if (activity < 0 && errno != EINTR) { + LogErr() << "select error: " << GET_ERROR(errno); + continue; + } + + if (activity == 0) { + // Timeout, no incoming connection + continue; + } + + if (FD_ISSET(_server_socket_fd, &readfds)) { + struct sockaddr_in client_addr {}; + socklen_t client_addr_len = sizeof(client_addr); + + _client_socket_fd = + accept(_server_socket_fd, (struct sockaddr*)&client_addr, &client_addr_len); + if (_client_socket_fd < 0) { + if (_should_exit) { + return; + } + LogErr() << "accept error: " << GET_ERROR(errno); + continue; + } + + _recv_thread = std::make_unique(&TcpServerConnection::receive, this); + } + } +} + +void TcpServerConnection::receive() +{ + char buffer[2048]; + + while (!_should_exit) { + if (!_is_ok) { + // LogErr() << "TCP receive error, trying to reconnect..."; + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + + const auto recv_len = recv(_client_socket_fd, buffer, sizeof(buffer), 0); + + if (recv_len == 0) { + _is_ok = false; + continue; + } + + if (recv_len < 0) { + _is_ok = false; + continue; + } + + _mavlink_receiver->set_new_datagram(buffer, static_cast(recv_len)); + + // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while. + while (_mavlink_receiver->parse_message()) { + receive_message(_mavlink_receiver->get_last_message(), this); + } + } +} + +} // namespace mavsdk \ No newline at end of file diff --git a/src/mavsdk/core/tcp_server_connection.h b/src/mavsdk/core/tcp_server_connection.h new file mode 100644 index 0000000000..7b3993878a --- /dev/null +++ b/src/mavsdk/core/tcp_server_connection.h @@ -0,0 +1,39 @@ +#pragma once + +#include "connection.h" + +#include +#include +#include + +namespace mavsdk { + +class TcpServerConnection : public Connection { +public: + TcpServerConnection( + Connection::ReceiverCallback receiver_callback, + std::string local_ip, + int local_port, + ForwardingOption forwarding_option); + ~TcpServerConnection(); + + ConnectionResult start(); + ConnectionResult stop(); + bool send_message(const mavlink_message_t& message); + +private: + Connection::ReceiverCallback _receiver_callback; + std::string _local_ip; + int _local_port; + int _server_socket_fd; + int _client_socket_fd; + std::unique_ptr _accept_thread; + std::unique_ptr _recv_thread; + std::atomic _should_exit; + std::atomic _is_ok; + + void accept_client(); + void receive(); +}; + +} // namespace mavsdk From 49851231b90aa5e0f72445a33c4b06ae179b0a35 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Jun 2024 21:29:15 +0200 Subject: [PATCH 05/17] system_tests: use TCP server at least once --- src/system_tests/telemetry_subscription.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/system_tests/telemetry_subscription.cpp b/src/system_tests/telemetry_subscription.cpp index a67aa45c14..44edba2868 100644 --- a/src/system_tests/telemetry_subscription.cpp +++ b/src/system_tests/telemetry_subscription.cpp @@ -14,9 +14,11 @@ TEST(SystemTest, TelemetrySubscription) Mavsdk mavsdk_autopilot{Mavsdk::Configuration{Mavsdk::ComponentType::Autopilot}}; - ASSERT_EQ(mavsdk_groundstation.add_any_connection("udp://:17000"), ConnectionResult::Success); ASSERT_EQ( - mavsdk_autopilot.add_any_connection("udp://127.0.0.1:17000"), ConnectionResult::Success); + mavsdk_groundstation.add_any_connection("tcpin://0.0.0.0:13000"), + ConnectionResult::Success); + ASSERT_EQ( + mavsdk_autopilot.add_any_connection("tcpout://127.0.0.1:13000"), ConnectionResult::Success); auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()}; From 100ce65db2985c1b27ee405f724eba68c8d1e6dc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 11:08:36 +0200 Subject: [PATCH 06/17] core: use only one receiver thread for TCP server We only do accept() again once we had an error and need to re-connect. --- src/mavsdk/core/tcp_client_connection.h | 2 +- src/mavsdk/core/tcp_server_connection.cpp | 62 ++++++++--------------- src/mavsdk/core/tcp_server_connection.h | 24 ++++----- 3 files changed, 34 insertions(+), 54 deletions(-) diff --git a/src/mavsdk/core/tcp_client_connection.h b/src/mavsdk/core/tcp_client_connection.h index b243cb72b7..2dfe8ca433 100644 --- a/src/mavsdk/core/tcp_client_connection.h +++ b/src/mavsdk/core/tcp_client_connection.h @@ -19,7 +19,7 @@ namespace mavsdk { class TcpClientConnection : public Connection { public: - explicit TcpClientConnection( + TcpClientConnection( Connection::ReceiverCallback receiver_callback, std::string remote_ip, int remote_port, diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp index 8b7d74b71a..d942413673 100644 --- a/src/mavsdk/core/tcp_server_connection.cpp +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -31,12 +31,8 @@ TcpServerConnection::TcpServerConnection( int local_port, ForwardingOption forwarding_option) : Connection(std::move(receiver_callback), forwarding_option), - _local_ip(local_ip), - _local_port(local_port), - _server_socket_fd(-1), - _client_socket_fd(-1), - _should_exit(false), - _is_ok(false) + _local_ip(std::move(local_ip)), + _local_port(local_port) {} TcpServerConnection::~TcpServerConnection() @@ -64,12 +60,13 @@ ConnectionResult TcpServerConnection::start() return ConnectionResult::SocketError; } - struct sockaddr_in server_addr {}; + sockaddr_in server_addr{}; server_addr.sin_family = AF_INET; server_addr.sin_addr.s_addr = INADDR_ANY; server_addr.sin_port = htons(_local_port); - if (bind(_server_socket_fd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { + if (bind(_server_socket_fd, reinterpret_cast(&server_addr), sizeof(server_addr)) < + 0) { LogErr() << "bind error: " << GET_ERROR(errno); return ConnectionResult::SocketError; } @@ -79,8 +76,8 @@ ConnectionResult TcpServerConnection::start() return ConnectionResult::SocketError; } - _is_ok = true; - _accept_thread = std::make_unique(&TcpServerConnection::accept_client, this); + _accept_receive_thread = + std::make_unique(&TcpServerConnection::accept_client, this); return ConnectionResult::Success; } @@ -100,12 +97,9 @@ ConnectionResult TcpServerConnection::stop() WSACleanup(); #endif - if (_accept_thread && _accept_thread->joinable()) { - _accept_thread->join(); - } - - if (_recv_thread && _recv_thread->joinable()) { - _recv_thread->join(); + if (_accept_receive_thread && _accept_receive_thread->joinable()) { + _accept_receive_thread->join(); + _accept_receive_thread.reset(); } // We need to stop this after stopping the receive thread, otherwise @@ -117,10 +111,6 @@ ConnectionResult TcpServerConnection::stop() bool TcpServerConnection::send_message(const mavlink_message_t& message) { - if (!_is_ok) { - return false; - } - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); @@ -143,22 +133,21 @@ bool TcpServerConnection::send_message(const mavlink_message_t& message) void TcpServerConnection::accept_client() { - fd_set readfds; - struct timeval timeout; - // Set server socket to non-blocking int flags = fcntl(_server_socket_fd, F_GETFL, 0); fcntl(_server_socket_fd, F_SETFL, flags | O_NONBLOCK); while (!_should_exit) { + fd_set readfds; FD_ZERO(&readfds); FD_SET(_server_socket_fd, &readfds); // Set timeout to 1 second + timeval timeout; timeout.tv_sec = 1; timeout.tv_usec = 0; - int activity = select(_server_socket_fd + 1, &readfds, nullptr, nullptr, &timeout); + const int activity = select(_server_socket_fd + 1, &readfds, nullptr, nullptr, &timeout); if (activity < 0 && errno != EINTR) { LogErr() << "select error: " << GET_ERROR(errno); @@ -171,11 +160,11 @@ void TcpServerConnection::accept_client() } if (FD_ISSET(_server_socket_fd, &readfds)) { - struct sockaddr_in client_addr {}; + sockaddr_in client_addr{}; socklen_t client_addr_len = sizeof(client_addr); - _client_socket_fd = - accept(_server_socket_fd, (struct sockaddr*)&client_addr, &client_addr_len); + _client_socket_fd = accept( + _server_socket_fd, reinterpret_cast(&client_addr), &client_addr_len); if (_client_socket_fd < 0) { if (_should_exit) { return; @@ -184,35 +173,28 @@ void TcpServerConnection::accept_client() continue; } - _recv_thread = std::make_unique(&TcpServerConnection::receive, this); + receive(); } } } void TcpServerConnection::receive() { - char buffer[2048]; + std::array buffer{}; while (!_should_exit) { - if (!_is_ok) { - // LogErr() << "TCP receive error, trying to reconnect..."; - std::this_thread::sleep_for(std::chrono::seconds(1)); - continue; - } - - const auto recv_len = recv(_client_socket_fd, buffer, sizeof(buffer), 0); + const auto recv_len = recv(_client_socket_fd, buffer.data(), buffer.size(), 0); if (recv_len == 0) { - _is_ok = false; continue; } if (recv_len < 0) { - _is_ok = false; - continue; + LogErr() << "recv failed: " << GET_ERROR(errno); + return; } - _mavlink_receiver->set_new_datagram(buffer, static_cast(recv_len)); + _mavlink_receiver->set_new_datagram(buffer.data(), static_cast(recv_len)); // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while. while (_mavlink_receiver->parse_message()) { diff --git a/src/mavsdk/core/tcp_server_connection.h b/src/mavsdk/core/tcp_server_connection.h index 7b3993878a..3681afc04c 100644 --- a/src/mavsdk/core/tcp_server_connection.h +++ b/src/mavsdk/core/tcp_server_connection.h @@ -15,25 +15,23 @@ class TcpServerConnection : public Connection { std::string local_ip, int local_port, ForwardingOption forwarding_option); - ~TcpServerConnection(); + ~TcpServerConnection() override; - ConnectionResult start(); - ConnectionResult stop(); - bool send_message(const mavlink_message_t& message); + ConnectionResult start() override; + ConnectionResult stop() override; + bool send_message(const mavlink_message_t& message) override; private: + void accept_client(); + void receive(); + Connection::ReceiverCallback _receiver_callback; std::string _local_ip; int _local_port; - int _server_socket_fd; - int _client_socket_fd; - std::unique_ptr _accept_thread; - std::unique_ptr _recv_thread; - std::atomic _should_exit; - std::atomic _is_ok; - - void accept_client(); - void receive(); + int _server_socket_fd{-1}; + int _client_socket_fd{-1}; + std::unique_ptr _accept_receive_thread; + std::atomic _should_exit{false}; }; } // namespace mavsdk From d536b579620c70e1e09c5f7229e846b1dd9e502c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 14:22:35 +0200 Subject: [PATCH 07/17] core: print deprecation warning for udp://, tcp:// --- src/mavsdk/core/cli_arg.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mavsdk/core/cli_arg.cpp b/src/mavsdk/core/cli_arg.cpp index 7626102379..981e8c3807 100644 --- a/src/mavsdk/core/cli_arg.cpp +++ b/src/mavsdk/core/cli_arg.cpp @@ -25,6 +25,7 @@ bool CliArg::parse(const std::string& uri) const std::string delimiter = "://"; if (uri.find(udp + delimiter) == 0) { + LogWarn() << "Connection using udp:// is deprecated, please use udpin:// or udpout://"; return parse_udp(uri.substr(udp.size() + delimiter.size())); } @@ -37,6 +38,7 @@ bool CliArg::parse(const std::string& uri) } if (uri.find(tcp + delimiter) == 0) { + LogWarn() << "Connection using tcp:// is deprecated, please use tcpin:// or tcpout://"; return parse_tcp(uri.substr(tcp.size() + delimiter.size())); } From 585ef300269353e6a7365b7280a943b7595b5950 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 15:59:08 +0200 Subject: [PATCH 08/17] core: remove add_udp_connection reference --- src/mavsdk/core/include/mavsdk/connection_result.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/core/include/mavsdk/connection_result.h b/src/mavsdk/core/include/mavsdk/connection_result.h index 6e9a45e7cd..839db2636d 100644 --- a/src/mavsdk/core/include/mavsdk/connection_result.h +++ b/src/mavsdk/core/include/mavsdk/connection_result.h @@ -10,8 +10,8 @@ namespace mavsdk { /** * @brief Result type returned when adding a connection. * - * **Note**: Mavsdk does not throw exceptions. Instead a result of this type will be - * returned when you add a connection: add_udp_connection(). + * **Note**: Mavsdk does not throw exceptions. Instead a result of this type + * will be returned. */ enum class ConnectionResult { Success = 0, /**< @brief %Connection succeeded. */ From b9d2562959022c761edc13836ee28dc37aa088aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 15:59:27 +0200 Subject: [PATCH 09/17] examples: use add_any_connection --- examples/ftp_server/ftp_server.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/examples/ftp_server/ftp_server.cpp b/examples/ftp_server/ftp_server.cpp index b04d78dff3..e32d450a73 100644 --- a/examples/ftp_server/ftp_server.cpp +++ b/examples/ftp_server/ftp_server.cpp @@ -15,21 +15,18 @@ using namespace mavsdk; void usage(const std::string& bin_name) { - std::cerr << "Usage : " << bin_name << " \n" - << '\n' - << " Start mavlink FTP server on \n" - << " sending heartbeats to :\n"; + std::cerr << "Usage : " << bin_name << " " << std::endl; } int main(int argc, char** argv) { - if (argc != 4) { + if (argc != 3) { usage(argv[0]); return 1; } Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::CompanionComputer}}; - ConnectionResult connection_result = mavsdk.setup_udp_remote(argv[1], std::stoi(argv[2])); + ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]); if (connection_result != ConnectionResult::Success) { std::cerr << "Error setting up Mavlink FTP server.\n"; return 1; @@ -37,12 +34,12 @@ int main(int argc, char** argv) auto component = mavsdk.server_component(); auto ftp_server = FtpServer{component}; - ftp_server.set_root_dir(argv[3]); + ftp_server.set_root_dir(argv[2]); - std::cout << "Mavlink FTP server running.\n" - << '\n' - << "Remote: " << argv[1] << ":" << argv[2] << '\n' - << "Component ID: " << std::to_string(component->component_id()) << '\n'; + std::cout << "MAVLink FTP server running\n" + << " connection: " << argv[1] << '\n' + << " directory: " << argv[2] << '\n' + << " component ID: " << std::to_string(component->component_id()) << std::endl; while (true) { std::this_thread::sleep_for(std::chrono::seconds(1)); From 605ed02461cdd6bd2ebf67063ae7652f649ec09b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 15:59:43 +0200 Subject: [PATCH 10/17] integration_tests: use add_any_connection --- src/integration_tests/action_goto.cpp | 2 +- src/integration_tests/action_hold.cpp | 2 +- src/integration_tests/action_hover_async.cpp | 2 +- src/integration_tests/action_hover_sync.cpp | 4 ++-- src/integration_tests/action_takeoff_and_kill.cpp | 2 +- .../action_transition_multicopter_fixedwing.cpp | 2 +- src/integration_tests/calibration.cpp | 14 +++++++------- src/integration_tests/camera_capture_info.cpp | 2 +- src/integration_tests/camera_format.cpp | 2 +- src/integration_tests/camera_mode.cpp | 4 ++-- src/integration_tests/camera_reset_settings.cpp | 2 +- src/integration_tests/camera_settings.cpp | 8 ++++---- src/integration_tests/camera_status.cpp | 2 +- src/integration_tests/camera_take_photo.cpp | 2 +- .../camera_take_photo_interval.cpp | 2 +- src/integration_tests/follow_me.cpp | 4 ++-- src/integration_tests/geofence.cpp | 2 +- src/integration_tests/gimbal.cpp | 8 ++++---- src/integration_tests/info.cpp | 2 +- src/integration_tests/log_files.cpp | 6 +++--- src/integration_tests/logging.cpp | 2 +- src/integration_tests/mavlink_passthrough.cpp | 2 +- src/integration_tests/mission.cpp | 2 +- src/integration_tests/mission_cancellation.cpp | 4 ++-- src/integration_tests/mission_change_speed.cpp | 2 +- .../mission_raw_import_and_fly.cpp | 4 ++-- .../mission_raw_mission_changed.cpp | 2 +- src/integration_tests/mission_rtl.cpp | 2 +- src/integration_tests/mission_set_current.cpp | 2 +- src/integration_tests/mission_takeoff_land.cpp | 2 +- src/integration_tests/mission_transfer_lossy.cpp | 2 +- src/integration_tests/mission_transition.cpp | 2 +- src/integration_tests/offboard_acceleration.cpp | 2 +- src/integration_tests/offboard_attitude.cpp | 2 +- src/integration_tests/offboard_position.cpp | 2 +- src/integration_tests/offboard_velocity.cpp | 4 ++-- src/integration_tests/param.cpp | 8 ++++---- src/integration_tests/statustext.cpp | 5 +++-- src/integration_tests/system_connection_async.cpp | 2 +- src/integration_tests/system_multi_components.cpp | 2 +- src/integration_tests/telemetry_async.cpp | 2 +- src/integration_tests/telemetry_gps_origin.cpp | 2 +- src/integration_tests/telemetry_health.cpp | 2 +- src/integration_tests/telemetry_modes.cpp | 2 +- src/integration_tests/telemetry_sync.cpp | 2 +- 45 files changed, 70 insertions(+), 69 deletions(-) diff --git a/src/integration_tests/action_goto.cpp b/src/integration_tests/action_goto.cpp index e844487ccd..5ade6ca98f 100644 --- a/src/integration_tests/action_goto.cpp +++ b/src/integration_tests/action_goto.cpp @@ -11,7 +11,7 @@ TEST(SitlTest, PX4ActionGoto) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/action_hold.cpp b/src/integration_tests/action_hold.cpp index 1582f1b85b..40bde9ee7a 100644 --- a/src/integration_tests/action_hold.cpp +++ b/src/integration_tests/action_hold.cpp @@ -11,7 +11,7 @@ TEST(SitlTest, PX4ActionHold) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/action_hover_async.cpp b/src/integration_tests/action_hover_async.cpp index a2d4749ab6..92d1bc02c1 100644 --- a/src/integration_tests/action_hover_async.cpp +++ b/src/integration_tests/action_hover_async.cpp @@ -11,7 +11,7 @@ TEST(SitlTest, ActionHoverAsync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); auto system = std::shared_ptr{nullptr}; diff --git a/src/integration_tests/action_hover_sync.cpp b/src/integration_tests/action_hover_sync.cpp index 670abfbe02..fc1a78470d 100644 --- a/src/integration_tests/action_hover_sync.cpp +++ b/src/integration_tests/action_hover_sync.cpp @@ -48,7 +48,7 @@ void takeoff_and_hover_at_altitude(float altitude_m) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -125,7 +125,7 @@ void takeoff_and_hover_at_altitude_apm(float altitude_m) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/action_takeoff_and_kill.cpp b/src/integration_tests/action_takeoff_and_kill.cpp index ef76037cd8..9ccf61c09a 100644 --- a/src/integration_tests/action_takeoff_and_kill.cpp +++ b/src/integration_tests/action_takeoff_and_kill.cpp @@ -10,7 +10,7 @@ using namespace mavsdk; TEST(SitlTest, ActionTakeoffAndKill) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ASSERT_EQ(mavsdk.add_udp_connection(), ConnectionResult::Success); + ASSERT_EQ(mavsdk.add_any_connection("udpin://0.0.0.0:14540"), ConnectionResult::Success); { LogInfo() << "Waiting to discover vehicle"; diff --git a/src/integration_tests/action_transition_multicopter_fixedwing.cpp b/src/integration_tests/action_transition_multicopter_fixedwing.cpp index 8544fb3af0..7b0054ed20 100644 --- a/src/integration_tests/action_transition_multicopter_fixedwing.cpp +++ b/src/integration_tests/action_transition_multicopter_fixedwing.cpp @@ -12,7 +12,7 @@ TEST(SitlTest, PX4ActionTransitionSync_standard_vtol) // Init & connect Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/calibration.cpp b/src/integration_tests/calibration.cpp index 63dae5d3d7..1266e3ae8d 100644 --- a/src/integration_tests/calibration.cpp +++ b/src/integration_tests/calibration.cpp @@ -18,7 +18,7 @@ TEST(HardwareTest, CalibrationGyro) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -47,7 +47,7 @@ TEST(HardwareTest, CalibrationAccelerometer) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -76,7 +76,7 @@ TEST(HardwareTest, CalibrationMagnetometer) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -105,7 +105,7 @@ TEST(HardwareTest, CalibrationLevelHorizon) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -134,7 +134,7 @@ TEST(HardwareTest, CalibrationGimbalAccelerometer) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -163,7 +163,7 @@ TEST(HardwareTest, CalibrationGyroWithTelemetry) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -206,7 +206,7 @@ TEST(HardwareTest, CalibrationGyroCancelled) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_capture_info.cpp b/src/integration_tests/camera_capture_info.cpp index 137135fbd3..0c0b44ee1b 100644 --- a/src/integration_tests/camera_capture_info.cpp +++ b/src/integration_tests/camera_capture_info.cpp @@ -12,7 +12,7 @@ TEST(CameraTest, CaptureInfo) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_format.cpp b/src/integration_tests/camera_format.cpp index 5c9b1715ce..b0a98baa9e 100644 --- a/src/integration_tests/camera_format.cpp +++ b/src/integration_tests/camera_format.cpp @@ -11,7 +11,7 @@ TEST(CameraTest, Format) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_mode.cpp b/src/integration_tests/camera_mode.cpp index de20faed50..a2b6afebcf 100644 --- a/src/integration_tests/camera_mode.cpp +++ b/src/integration_tests/camera_mode.cpp @@ -11,7 +11,7 @@ TEST(CameraTest, SetModeSync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -50,7 +50,7 @@ TEST(CameraTest, SetModeAsync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_reset_settings.cpp b/src/integration_tests/camera_reset_settings.cpp index 5f602b540c..bd4f96ec1c 100644 --- a/src/integration_tests/camera_reset_settings.cpp +++ b/src/integration_tests/camera_reset_settings.cpp @@ -11,7 +11,7 @@ TEST(CameraTest, ResetSettings) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_settings.cpp b/src/integration_tests/camera_settings.cpp index a77e7bc7f5..145a3dda85 100644 --- a/src/integration_tests/camera_settings.cpp +++ b/src/integration_tests/camera_settings.cpp @@ -38,7 +38,7 @@ TEST(CameraTest, ShowSettingsAndOptions) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -127,7 +127,7 @@ TEST(CameraTest, SetSettings) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult connection_ret = mavsdk.add_udp_connection(); + ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(connection_ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -265,7 +265,7 @@ TEST(CameraTest, SubscribeCurrentSettings) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult connection_ret = mavsdk.add_udp_connection(); + ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(connection_ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -329,7 +329,7 @@ TEST(CameraTest, SubscribePossibleSettings) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult connection_ret = mavsdk.add_udp_connection(); + ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(connection_ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_status.cpp b/src/integration_tests/camera_status.cpp index 742c34fe60..eb43730b2a 100644 --- a/src/integration_tests/camera_status.cpp +++ b/src/integration_tests/camera_status.cpp @@ -13,7 +13,7 @@ TEST(CameraTest, Status) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_take_photo.cpp b/src/integration_tests/camera_take_photo.cpp index cc59d8103e..e5d1e94127 100644 --- a/src/integration_tests/camera_take_photo.cpp +++ b/src/integration_tests/camera_take_photo.cpp @@ -56,7 +56,7 @@ TEST(CameraTest, TakePhotosMultiple) const int num_photos_to_take = 3; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/camera_take_photo_interval.cpp b/src/integration_tests/camera_take_photo_interval.cpp index 9af4b65053..ef4d97f1b7 100644 --- a/src/integration_tests/camera_take_photo_interval.cpp +++ b/src/integration_tests/camera_take_photo_interval.cpp @@ -19,7 +19,7 @@ TEST(CameraTest, TakePhotoInterval) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/follow_me.cpp b/src/integration_tests/follow_me.cpp index 3f9168d1dc..211340ab8d 100644 --- a/src/integration_tests/follow_me.cpp +++ b/src/integration_tests/follow_me.cpp @@ -33,7 +33,7 @@ TEST(SitlTest, PX4FollowMeOneLocation) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. @@ -116,7 +116,7 @@ TEST(SitlTest, PX4FollowMeMultiLocationWithConfig) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/geofence.cpp b/src/integration_tests/geofence.cpp index 6a45b35bcc..de99a038c2 100644 --- a/src/integration_tests/geofence.cpp +++ b/src/integration_tests/geofence.cpp @@ -13,7 +13,7 @@ TEST(SitlTest, PX4GeofenceInclusion) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/gimbal.cpp b/src/integration_tests/gimbal.cpp index a375e50740..44e60a362a 100644 --- a/src/integration_tests/gimbal.cpp +++ b/src/integration_tests/gimbal.cpp @@ -25,7 +25,7 @@ TEST(SitlTestGimbal, GimbalMove) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -74,7 +74,7 @@ TEST(SitlTestGimbal, GimbalAngles) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -126,7 +126,7 @@ TEST(SitlTestGimbal, GimbalTakeoffAndMove) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -240,7 +240,7 @@ TEST(SitlTestGimbal, GimbalROIOffboard) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/info.cpp b/src/integration_tests/info.cpp index 5191b7f76e..353c4afd87 100644 --- a/src/integration_tests/info.cpp +++ b/src/integration_tests/info.cpp @@ -9,7 +9,7 @@ TEST(SitlTest, PX4Info) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/log_files.cpp b/src/integration_tests/log_files.cpp index 073222b74a..4a4bc99cb4 100644 --- a/src/integration_tests/log_files.cpp +++ b/src/integration_tests/log_files.cpp @@ -14,7 +14,7 @@ TEST(HardwareTest, LogFiles) Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; // ConnectionResult ret = mavsdk.add_serial_connection("/dev/ttyACM0"); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -69,7 +69,7 @@ TEST(HardwareTest, LogFilesDownloadFailsIfPathIsDirectory) Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; // ConnectionResult ret = mavsdk.add_serial_connection("/dev/ttyACM0"); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -125,7 +125,7 @@ TEST(HardwareTest, LogFilesDownloadFailsIfFileAlreadyExists) Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; // ConnectionResult ret = mavsdk.add_serial_connection("/dev/ttyACM0"); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/logging.cpp b/src/integration_tests/logging.cpp index 4132c1da80..db80f55c05 100644 --- a/src/integration_tests/logging.cpp +++ b/src/integration_tests/logging.cpp @@ -9,7 +9,7 @@ TEST(SitlTest, PX4Logging) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/mavlink_passthrough.cpp b/src/integration_tests/mavlink_passthrough.cpp index f1b62404b0..1b568c4199 100644 --- a/src/integration_tests/mavlink_passthrough.cpp +++ b/src/integration_tests/mavlink_passthrough.cpp @@ -10,7 +10,7 @@ using namespace mavsdk; TEST(SitlTest, PX4MavlinkPassthrough) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ASSERT_EQ(mavsdk.add_udp_connection(), ConnectionResult::Success); + ASSERT_EQ(mavsdk.add_any_connection("udpin://0.0.0.0:14540"), ConnectionResult::Success); { LogInfo() << "Waiting to discover vehicle"; diff --git a/src/integration_tests/mission.cpp b/src/integration_tests/mission.cpp index 46949b56e0..26351d31ca 100644 --- a/src/integration_tests/mission.cpp +++ b/src/integration_tests/mission.cpp @@ -54,7 +54,7 @@ TEST(SitlTest, PX4MissionAddWaypointsAndFly) } }); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); auto status = future_result.wait_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/mission_cancellation.cpp b/src/integration_tests/mission_cancellation.cpp index 93958cf360..4d17082c3c 100644 --- a/src/integration_tests/mission_cancellation.cpp +++ b/src/integration_tests/mission_cancellation.cpp @@ -26,7 +26,7 @@ TEST(SitlTest, PX4MissionUploadCancellation) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -82,7 +82,7 @@ TEST(SitlTest, PX4MissionDownloadCancellation) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/mission_change_speed.cpp b/src/integration_tests/mission_change_speed.cpp index a60c23c946..412ff776e0 100644 --- a/src/integration_tests/mission_change_speed.cpp +++ b/src/integration_tests/mission_change_speed.cpp @@ -24,7 +24,7 @@ TEST(SitlTest, PX4MissionChangeSpeed) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/mission_raw_import_and_fly.cpp b/src/integration_tests/mission_raw_import_and_fly.cpp index bb47df6773..275c3097fb 100644 --- a/src/integration_tests/mission_raw_import_and_fly.cpp +++ b/src/integration_tests/mission_raw_import_and_fly.cpp @@ -30,7 +30,7 @@ TEST(SitlTest, PX4MissionRawImportAndFly) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -91,7 +91,7 @@ TEST(SitlTest, APMissionRawImportAndFly) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/mission_raw_mission_changed.cpp b/src/integration_tests/mission_raw_mission_changed.cpp index 1dd35a5a95..d112c31cca 100644 --- a/src/integration_tests/mission_raw_mission_changed.cpp +++ b/src/integration_tests/mission_raw_mission_changed.cpp @@ -18,7 +18,7 @@ TEST(SitlTest, PX4MissionRawMissionChanged) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/mission_rtl.cpp b/src/integration_tests/mission_rtl.cpp index 545af66246..c9f9eedb0c 100644 --- a/src/integration_tests/mission_rtl.cpp +++ b/src/integration_tests/mission_rtl.cpp @@ -52,7 +52,7 @@ void do_mission_with_rtl(float mission_altitude_m, float return_altitude_m) } }); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); auto status = future_result.wait_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/mission_set_current.cpp b/src/integration_tests/mission_set_current.cpp index 32e67ab7a8..4cf3876207 100644 --- a/src/integration_tests/mission_set_current.cpp +++ b/src/integration_tests/mission_set_current.cpp @@ -20,7 +20,7 @@ TEST(SitlTest, PX4MissionSetCurrent) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/mission_takeoff_land.cpp b/src/integration_tests/mission_takeoff_land.cpp index 1cc7247cba..a16c98742f 100644 --- a/src/integration_tests/mission_takeoff_land.cpp +++ b/src/integration_tests/mission_takeoff_land.cpp @@ -32,7 +32,7 @@ TEST(SitlTest, MissionTakeoffAndLand) } }); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); auto status = future_result.wait_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/mission_transfer_lossy.cpp b/src/integration_tests/mission_transfer_lossy.cpp index 80501b9ea0..cb7a26af63 100644 --- a/src/integration_tests/mission_transfer_lossy.cpp +++ b/src/integration_tests/mission_transfer_lossy.cpp @@ -20,7 +20,7 @@ static std::uniform_real_distribution distribution(0.0, 1.0); TEST(SitlTest, PX4MissionTransferLossy) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ASSERT_EQ(mavsdk.add_udp_connection(), ConnectionResult::Success); + ASSERT_EQ(mavsdk.add_any_connection("udpin://0.0.0.0:14540"), ConnectionResult::Success); { LogInfo() << "Waiting to discover vehicle"; diff --git a/src/integration_tests/mission_transition.cpp b/src/integration_tests/mission_transition.cpp index 725af1cec8..dfde41b234 100644 --- a/src/integration_tests/mission_transition.cpp +++ b/src/integration_tests/mission_transition.cpp @@ -32,7 +32,7 @@ TEST(SitlTest, MissionTakeoffTransitionAndLand_standard_vtol) } }); - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); auto status = future_result.wait_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/offboard_acceleration.cpp b/src/integration_tests/offboard_acceleration.cpp index 73b9f291d1..0492f15f70 100644 --- a/src/integration_tests/offboard_acceleration.cpp +++ b/src/integration_tests/offboard_acceleration.cpp @@ -13,7 +13,7 @@ TEST(SitlTest, PX4OffboardAccelerationNED) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/offboard_attitude.cpp b/src/integration_tests/offboard_attitude.cpp index 253ead23ac..48fa148e4e 100644 --- a/src/integration_tests/offboard_attitude.cpp +++ b/src/integration_tests/offboard_attitude.cpp @@ -20,7 +20,7 @@ TEST(SitlTestDisabled, OffboardAttitudeRate) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/offboard_position.cpp b/src/integration_tests/offboard_position.cpp index e65ba8ad00..130f59e7c6 100644 --- a/src/integration_tests/offboard_position.cpp +++ b/src/integration_tests/offboard_position.cpp @@ -14,7 +14,7 @@ TEST(SitlTest, OffboardPositionNED) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/offboard_velocity.cpp b/src/integration_tests/offboard_velocity.cpp index b6e80fb430..64b0bd3f99 100644 --- a/src/integration_tests/offboard_velocity.cpp +++ b/src/integration_tests/offboard_velocity.cpp @@ -14,7 +14,7 @@ TEST(SitlTest, OffboardVelocityNED) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. @@ -136,7 +136,7 @@ TEST(SitlTest, OffboardVelocityBody) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ConnectionResult::Success, ret); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/param.cpp b/src/integration_tests/param.cpp index 94ae31f0f4..d709366df9 100644 --- a/src/integration_tests/param.cpp +++ b/src/integration_tests/param.cpp @@ -10,7 +10,7 @@ TEST(SitlTest, PX4ParamSad) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -48,7 +48,7 @@ TEST(SitlTest, PX4ParamHappy) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -129,7 +129,7 @@ TEST(SitlTest, GetAllParams) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. @@ -170,7 +170,7 @@ TEST(SitlTest, APParam) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); // Wait for system to connect via heartbeat. diff --git a/src/integration_tests/statustext.cpp b/src/integration_tests/statustext.cpp index d532d54b94..a829038585 100644 --- a/src/integration_tests/statustext.cpp +++ b/src/integration_tests/statustext.cpp @@ -16,10 +16,11 @@ static const auto type = ServerUtility::StatusTextType::Info; TEST(StatusTextTest, TestServer) { Mavsdk mavsdk_gcs{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ASSERT_EQ(mavsdk_gcs.add_udp_connection(24550), ConnectionResult::Success); + ASSERT_EQ(mavsdk_gcs.add_any_connection("udpin://0.0.0.0:24550"), ConnectionResult::Success); Mavsdk mavsdk_onboard{Mavsdk::Configuration{Mavsdk::ComponentType::CompanionComputer}}; - ASSERT_EQ(mavsdk_onboard.setup_udp_remote("127.0.0.1", 24550), ConnectionResult::Success); + ASSERT_EQ( + mavsdk_onboard.add_any_connection("udpout://127.0.0.1:24550"), ConnectionResult::Success); // Let the two connect to each other. std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/system_connection_async.cpp b/src/integration_tests/system_connection_async.cpp index f3f002e12f..8f1e6b65d8 100644 --- a/src/integration_tests/system_connection_async.cpp +++ b/src/integration_tests/system_connection_async.cpp @@ -16,7 +16,7 @@ TEST(SitlTest, SystemConnectionAsync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ASSERT_EQ(mavsdk.add_udp_connection(), ConnectionResult::Success); + ASSERT_EQ(mavsdk.add_any_connection("udpin://0.0.0.0:14540"), ConnectionResult::Success); mavsdk.subscribe_on_new_system([&mavsdk]() { const auto system = mavsdk.systems().at(0); diff --git a/src/integration_tests/system_multi_components.cpp b/src/integration_tests/system_multi_components.cpp index 4a4faaf8b5..31857c76d9 100644 --- a/src/integration_tests/system_multi_components.cpp +++ b/src/integration_tests/system_multi_components.cpp @@ -28,7 +28,7 @@ TEST(SitlTestMultiple, SystemMultipleComponents) Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; // For both Autopilot and Camera - ASSERT_EQ(mavsdk.add_udp_connection(), ConnectionResult::Success); + ASSERT_EQ(mavsdk.add_any_connection("udpin://0.0.0.0:14540"), ConnectionResult::Success); // FIXME: As components send Heartbeats at 1Hz, // lets wait until atleast 2 of them gets discovered. diff --git a/src/integration_tests/telemetry_async.cpp b/src/integration_tests/telemetry_async.cpp index 839c2d235d..0cfe15941e 100644 --- a/src/integration_tests/telemetry_async.cpp +++ b/src/integration_tests/telemetry_async.cpp @@ -62,7 +62,7 @@ TEST(SitlTest, PX4TelemetryAsync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(3)); diff --git a/src/integration_tests/telemetry_gps_origin.cpp b/src/integration_tests/telemetry_gps_origin.cpp index 7d465f3085..a70d9223c1 100644 --- a/src/integration_tests/telemetry_gps_origin.cpp +++ b/src/integration_tests/telemetry_gps_origin.cpp @@ -8,7 +8,7 @@ TEST(SitlTestDisabled, TelemetryGpsOrigin) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/telemetry_health.cpp b/src/integration_tests/telemetry_health.cpp index 86a6703ea0..1faa643f4a 100644 --- a/src/integration_tests/telemetry_health.cpp +++ b/src/integration_tests/telemetry_health.cpp @@ -12,7 +12,7 @@ TEST(SitlTest, TelemetryHealth) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/telemetry_modes.cpp b/src/integration_tests/telemetry_modes.cpp index 3fdcaebc33..7a9904261a 100644 --- a/src/integration_tests/telemetry_modes.cpp +++ b/src/integration_tests/telemetry_modes.cpp @@ -14,7 +14,7 @@ TEST(SitlTestDisabled, TelemetryFlightModes) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/src/integration_tests/telemetry_sync.cpp b/src/integration_tests/telemetry_sync.cpp index f1ffbc96d9..8f2f356fc3 100644 --- a/src/integration_tests/telemetry_sync.cpp +++ b/src/integration_tests/telemetry_sync.cpp @@ -9,7 +9,7 @@ TEST(SitlTest, PX4TelemetrySync) { Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult ret = mavsdk.add_udp_connection(); + ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); ASSERT_EQ(ret, ConnectionResult::Success); std::this_thread::sleep_for(std::chrono::seconds(2)); From b60d20f9d508aa16a64f8857c679edfc9844f649 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2024 20:48:16 +0200 Subject: [PATCH 11/17] core: follow up fixes --- src/mavsdk/core/cli_arg.cpp | 111 +++++++++++----------- src/mavsdk/core/cli_arg.h | 6 +- src/mavsdk/core/mavsdk_impl.cpp | 11 ++- src/mavsdk/core/tcp_client_connection.cpp | 8 +- 4 files changed, 71 insertions(+), 65 deletions(-) diff --git a/src/mavsdk/core/cli_arg.cpp b/src/mavsdk/core/cli_arg.cpp index 981e8c3807..0e5ad4ac4b 100644 --- a/src/mavsdk/core/cli_arg.cpp +++ b/src/mavsdk/core/cli_arg.cpp @@ -26,36 +26,37 @@ bool CliArg::parse(const std::string& uri) if (uri.find(udp + delimiter) == 0) { LogWarn() << "Connection using udp:// is deprecated, please use udpin:// or udpout://"; - return parse_udp(uri.substr(udp.size() + delimiter.size())); + return parse_udp(std::string_view(uri).substr(udp.size() + delimiter.size())); } if (uri.find(udpin + delimiter) == 0) { - return parse_udpin(uri.substr(udpin.size() + delimiter.size())); + return parse_udpin(std::string_view(uri).substr(udpin.size() + delimiter.size())); } if (uri.find(udpout + delimiter) == 0) { - return parse_udpout(uri.substr(udpout.size() + delimiter.size())); + return parse_udpout(std::string_view(uri).substr(udpout.size() + delimiter.size())); } if (uri.find(tcp + delimiter) == 0) { LogWarn() << "Connection using tcp:// is deprecated, please use tcpin:// or tcpout://"; - return parse_tcp(uri.substr(tcp.size() + delimiter.size())); + return parse_tcp(std::string_view(uri).substr(tcp.size() + delimiter.size())); } if (uri.find(tcpin + delimiter) == 0) { - return parse_tcpin(uri.substr(tcpin.size() + delimiter.size())); + return parse_tcpin(std::string_view(uri).substr(tcpin.size() + delimiter.size())); } if (uri.find(tcpout + delimiter) == 0) { - return parse_tcpout(uri.substr(tcpout.size() + delimiter.size())); + return parse_tcpout(std::string_view(uri).substr(tcpout.size() + delimiter.size())); } if (uri.find(serial + delimiter) == 0) { - return parse_serial(uri.substr(serial.size() + delimiter.size()), false); + return parse_serial(std::string_view(uri).substr(serial.size() + delimiter.size()), false); } if (uri.find(serial_flowcontrol + delimiter) == 0) { - return parse_serial(uri.substr(serial_flowcontrol.size() + delimiter.size()), true); + return parse_serial( + std::string_view(uri).substr(serial_flowcontrol.size() + delimiter.size()), true); } LogErr() << "Unknown protocol"; @@ -71,19 +72,19 @@ bool CliArg::parse_udp(const std::string_view rest) } protocol = Udp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - if (p->host.empty()) { - p->host = "0.0.0.0"; - p->mode = Udp::Mode::In; - } else if (p->host == "0.0.0.0") { - p->mode = Udp::Mode::In; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + if (p.host.empty()) { + p.host = "0.0.0.0"; + p.mode = Udp::Mode::In; + } else if (p.host == "0.0.0.0") { + p.mode = Udp::Mode::In; } else { - p->mode = Udp::Mode::Out; + p.mode = Udp::Mode::Out; } if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -99,12 +100,14 @@ bool CliArg::parse_udpin(const std::string_view rest) } protocol = Udp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - p->mode = Udp::Mode::In; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + p.mode = Udp::Mode::In; + p.host = rest.substr(0, pos); + p.mode = Udp::Mode::In; if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -120,18 +123,18 @@ bool CliArg::parse_udpout(const std::string_view rest) } protocol = Udp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - p->mode = Udp::Mode::Out; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + p.mode = Udp::Mode::Out; - if (p->host == "0.0.0.0") { + if (p.host == "0.0.0.0") { LogErr() << "0.0.0.0 is invalid for UDP out address. " "Can only listen on all interfaces, but not send."; return false; } if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -147,19 +150,19 @@ bool CliArg::parse_tcp(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - if (p->host.empty()) { - p->host = "0.0.0.0"; - p->mode = Tcp::Mode::In; - } else if (p->host == "0.0.0.0") { - p->mode = Tcp::Mode::In; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + if (p.host.empty()) { + p.host = "0.0.0.0"; + p.mode = Tcp::Mode::In; + } else if (p.host == "0.0.0.0") { + p.mode = Tcp::Mode::In; } else { - p->mode = Tcp::Mode::Out; + p.mode = Tcp::Mode::Out; } if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -175,12 +178,12 @@ bool CliArg::parse_tcpin(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - p->mode = Tcp::Mode::In; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + p.mode = Tcp::Mode::In; if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -196,18 +199,18 @@ bool CliArg::parse_tcpout(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get_if(&protocol); - p->host = rest.substr(0, pos); - p->mode = Tcp::Mode::Out; + auto p = std::get(protocol); + p.host = rest.substr(0, pos); + p.mode = Tcp::Mode::Out; - if (p->host == "0.0.0.0") { + if (p.host == "0.0.0.0") { LogErr() << "0.0.0.0 is invalid for TCP out address. " "Can only listen on all interfaces, but not send."; return false; } if (auto maybe_port = port_from_str(rest.substr(pos + 1))) { - p->port = maybe_port.value(); + p.port = maybe_port.value(); return true; } else { return false; @@ -228,8 +231,8 @@ std::optional CliArg::port_from_str(std::string_view str) bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled) { protocol = Serial{}; - auto p = std::get_if(&protocol); - p->flow_control_enabled = flow_control_enabled; + auto p = std::get(protocol); + p.flow_control_enabled = flow_control_enabled; const std::string delimiter = ":"; size_t pos = rest.find(delimiter); @@ -237,28 +240,28 @@ bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled return false; } - p->path = rest.substr(0, pos); + p.path = rest.substr(0, pos); - const auto path_is_only_numbers = std::all_of( - p->path.begin(), p->path.end(), [](unsigned char c) { return std::isdigit(c); }); + const auto path_is_only_numbers = + std::all_of(p.path.begin(), p.path.end(), [](unsigned char c) { return std::isdigit(c); }); if (path_is_only_numbers) { LogErr() << "Path can't be numbers only."; return false; } - if (p->path.find('/') == 0) { + if (p.path.find('/') == 0) { // A Linux/macOS path needs to start with '/'. - } else if (p->path.find("COM") == 0) { + } else if (p.path.find("COM") == 0) { // On Windows a path starting with 'COM' is ok but needs to be followed by digits. - for (const auto& digit : p->path.substr(3, p->path.length() - 3)) { + for (const auto& digit : p.path.substr(3, p.path.length() - 3)) { if (!std::isdigit(digit)) { LogErr() << "COM port number invalid."; return false; } } - if (p->path.length() == 3) { + if (p.path.length() == 3) { LogErr() << "COM port number missing"; return false; } @@ -282,7 +285,7 @@ bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled return false; } - p->baudrate = value; + p.baudrate = value; return true; } diff --git a/src/mavsdk/core/cli_arg.h b/src/mavsdk/core/cli_arg.h index 8ee0117952..c24f48e9e5 100644 --- a/src/mavsdk/core/cli_arg.h +++ b/src/mavsdk/core/cli_arg.h @@ -14,7 +14,8 @@ class CliArg { Unknown, In, Out, - } mode{Mode::Unknown}; + }; + Mode mode{Mode::Unknown}; std::string host{}; int port{}; }; @@ -24,7 +25,8 @@ class CliArg { Unknown, In, Out, - } mode{Mode::Unknown}; + }; + Mode mode{Mode::Unknown}; std::string host{}; int port{}; }; diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index baa6eb8a87..535b1cc639 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -471,17 +471,18 @@ std::pair MavsdkImpl::add_any_connec return std::visit( overloaded{ - [&](std::monostate) -> std::pair { + [](std::monostate) { // Should not happen anyway. - return {ConnectionResult::ConnectionUrlInvalid, Mavsdk::ConnectionHandle()}; + return std::pair{ + ConnectionResult::ConnectionUrlInvalid, Mavsdk::ConnectionHandle()}; }, - [&](CliArg::Udp& udp) -> std::pair { + [this, forwarding_option](const CliArg::Udp& udp) { return add_udp_connection(udp, forwarding_option); }, - [&](CliArg::Tcp& tcp) -> std::pair { + [this, forwarding_option](const CliArg::Tcp& tcp) { return add_tcp_connection(tcp, forwarding_option); }, - [&](CliArg::Serial& serial) -> std::pair { + [this, forwarding_option](const CliArg::Serial& serial) { return add_serial_connection( serial.path, serial.baudrate, serial.flow_control_enabled, forwarding_option); }}, diff --git a/src/mavsdk/core/tcp_client_connection.cpp b/src/mavsdk/core/tcp_client_connection.cpp index 40364ecba8..d6531c8142 100644 --- a/src/mavsdk/core/tcp_client_connection.cpp +++ b/src/mavsdk/core/tcp_client_connection.cpp @@ -1,6 +1,10 @@ #include "tcp_client_connection.h" #include "log.h" +#include +#include +#include + #ifdef WINDOWS #ifndef MINGW #pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library. @@ -14,10 +18,6 @@ #include // for close() #endif -#include -#include -#include - #ifndef WINDOWS #define GET_ERROR(_x) strerror(_x) #else From e1286f51cd3b2f561fac56da90adf1760657cf87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2024 20:23:04 +1200 Subject: [PATCH 12/17] core: take a reference, not a copy --- src/mavsdk/core/cli_arg.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/mavsdk/core/cli_arg.cpp b/src/mavsdk/core/cli_arg.cpp index 0e5ad4ac4b..c66511d396 100644 --- a/src/mavsdk/core/cli_arg.cpp +++ b/src/mavsdk/core/cli_arg.cpp @@ -72,7 +72,7 @@ bool CliArg::parse_udp(const std::string_view rest) } protocol = Udp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); if (p.host.empty()) { p.host = "0.0.0.0"; @@ -100,7 +100,7 @@ bool CliArg::parse_udpin(const std::string_view rest) } protocol = Udp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); p.mode = Udp::Mode::In; p.host = rest.substr(0, pos); @@ -123,7 +123,7 @@ bool CliArg::parse_udpout(const std::string_view rest) } protocol = Udp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); p.mode = Udp::Mode::Out; @@ -150,7 +150,7 @@ bool CliArg::parse_tcp(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); if (p.host.empty()) { p.host = "0.0.0.0"; @@ -178,7 +178,7 @@ bool CliArg::parse_tcpin(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); p.mode = Tcp::Mode::In; @@ -199,7 +199,7 @@ bool CliArg::parse_tcpout(const std::string_view rest) } protocol = Tcp{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.host = rest.substr(0, pos); p.mode = Tcp::Mode::Out; @@ -231,7 +231,7 @@ std::optional CliArg::port_from_str(std::string_view str) bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled) { protocol = Serial{}; - auto p = std::get(protocol); + auto& p = std::get(protocol); p.flow_control_enabled = flow_control_enabled; const std::string delimiter = ":"; From 430d70aaeaeaa7d17141f2d2a89845b83fec00ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Jul 2024 06:39:46 +1200 Subject: [PATCH 13/17] core: fix potential null dereference --- src/mavsdk/core/cli_arg_test.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/mavsdk/core/cli_arg_test.cpp b/src/mavsdk/core/cli_arg_test.cpp index a8eb7fa901..6881a763a4 100644 --- a/src/mavsdk/core/cli_arg_test.cpp +++ b/src/mavsdk/core/cli_arg_test.cpp @@ -346,6 +346,7 @@ TEST(CliArg, SerialWindowsComport) EXPECT_TRUE(ca.parse("serial://COM13:57600")); auto serial = std::get_if(&ca.protocol); + ASSERT_TRUE(serial); EXPECT_STREQ(serial->path.c_str(), "COM13"); EXPECT_EQ(serial->baudrate, 57600); EXPECT_EQ(serial->flow_control_enabled, false); @@ -372,4 +373,4 @@ TEST(CliArg, SerialWrong) EXPECT_FALSE(ca.parse("serial://SOM3:57600")); EXPECT_FALSE(ca.parse("serial://COM3:-1")); EXPECT_FALSE(ca.parse("serial://COM3")); -} \ No newline at end of file +} From 83c717a3b3cf8e87d56eed234b8ea10a9834b6b0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Jul 2024 06:41:34 +1200 Subject: [PATCH 14/17] core: fix include for Windows --- src/mavsdk/core/tcp_server_connection.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp index d942413673..6bd85dea78 100644 --- a/src/mavsdk/core/tcp_server_connection.cpp +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -2,7 +2,6 @@ #include "log.h" #include -#include #include #ifdef WINDOWS @@ -11,6 +10,7 @@ #endif #else #include +#include #include #include #include @@ -203,4 +203,4 @@ void TcpServerConnection::receive() } } -} // namespace mavsdk \ No newline at end of file +} // namespace mavsdk From 2dc66a2fdc00a95ad0abaa843913100704181f01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2024 10:44:13 +1200 Subject: [PATCH 15/17] core: get TCP server working on Windows --- src/mavsdk/core/tcp_server_connection.cpp | 33 +++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp index 6bd85dea78..aeba97accf 100644 --- a/src/mavsdk/core/tcp_server_connection.cpp +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -8,6 +8,9 @@ #ifndef MINGW #pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library. #endif +#include +#include +#include #else #include #include @@ -122,7 +125,8 @@ bool TcpServerConnection::send_message(const mavlink_message_t& message) auto flags = MSG_NOSIGNAL; #endif - const auto send_len = send(_client_socket_fd, buffer, buffer_len, flags); + const auto send_len = + send(_client_socket_fd, reinterpret_cast(buffer), buffer_len, flags); if (send_len != buffer_len) { LogErr() << "send failure: " << GET_ERROR(errno); @@ -133,9 +137,18 @@ bool TcpServerConnection::send_message(const mavlink_message_t& message) void TcpServerConnection::accept_client() { +#ifdef WINDOWS + // Set server socket to non-blocking + u_long iMode = 1; + int iResult = ioctlsocket(_server_socket_fd, FIONBIO, &iMode); + if (iResult != 0) { + LogErr() << "ioctlsocket failed with error: " << WSAGetLastError(); + } +#else // Set server socket to non-blocking int flags = fcntl(_server_socket_fd, F_GETFL, 0); fcntl(_server_socket_fd, F_SETFL, flags | O_NONBLOCK); +#endif while (!_should_exit) { fd_set readfds; @@ -182,9 +195,25 @@ void TcpServerConnection::receive() { std::array buffer{}; - while (!_should_exit) { + bool dataReceived = false; + while (!dataReceived && !_should_exit) { const auto recv_len = recv(_client_socket_fd, buffer.data(), buffer.size(), 0); +#ifdef WINDOWS + if (recv_len == SOCKET_ERROR) { + // On Windows, on the first try, select says there is something + // but recv doesn't succeed yet, and we just need to try again. + if (WSAGetLastError() == WSAEWOULDBLOCK) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + // And at the end, we get an abort that we can silently ignore. + if (WSAGetLastError() == WSAECONNABORTED) { + return; + } + } +#endif + if (recv_len == 0) { continue; } From 0a24376d9cfb9ce8c52cef2eb58f89971d2ca033 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2024 10:44:44 +1200 Subject: [PATCH 16/17] system_tests: fix test timing on Windows --- src/system_tests/ftp_download_file.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/system_tests/ftp_download_file.cpp b/src/system_tests/ftp_download_file.cpp index a29d75459a..4ee750d87d 100644 --- a/src/system_tests/ftp_download_file.cpp +++ b/src/system_tests/ftp_download_file.cpp @@ -250,7 +250,7 @@ TEST(SystemTest, FtpDownloadStopAndTryAgain) temp_dir_downloaded.string(), false, [&prom, &got_half](Ftp::Result result, Ftp::ProgressData progress_data) { - if (progress_data.bytes_transferred > 500) { + if (progress_data.bytes_transferred > 200) { got_half = true; } if (result != Ftp::Result::Next) { From b74706e61e5e998855959e80c34e269fa71e79a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2024 11:08:57 +1200 Subject: [PATCH 17/17] core: fix TCP server for macOS --- src/mavsdk/core/tcp_server_connection.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp index aeba97accf..a98121fd20 100644 --- a/src/mavsdk/core/tcp_server_connection.cpp +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -212,17 +212,23 @@ void TcpServerConnection::receive() return; } } +#else + if (recv_len < 0) { + // On macOS we presumably see the same thing, and have to try again. + if (errno == EAGAIN) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + + LogErr() << "recv failed: " << GET_ERROR(errno); + return; + } #endif if (recv_len == 0) { continue; } - if (recv_len < 0) { - LogErr() << "recv failed: " << GET_ERROR(errno); - return; - } - _mavlink_receiver->set_new_datagram(buffer.data(), static_cast(recv_len)); // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.