core: add raw connection

This adds a "raw connection" which accepts and delivers raw bytes. This
can be handy when a custom transport other than the included
UDP/TCP/serial needs to be used.

Author: julianoes

src/mavsdk/core/CMakeLists.txt

@@ -80,6 +80,7 @@ target_sources(mavsdk
     param_value.cpp
     ping.cpp
     plugin_impl_base.cpp
+    raw_connection.cpp
     serial_connection.cpp
     server_component.cpp
     server_component_impl.cpp

src/mavsdk/core/cli_arg.cpp

@@ -59,6 +59,11 @@ bool CliArg::parse(const std::string& uri)
             std::string_view(uri).substr(serial_flowcontrol.size() + delimiter.size()), true);
     }
 
+    const std::string raw = "raw";
+    if (uri.find(raw + delimiter) == 0) {
+        return parse_raw(std::string_view(uri).substr(raw.size() + delimiter.size()));
+    }
+
     LogErr() << "Unknown protocol";
     return false;
 }
@@ -293,4 +298,16 @@ bool CliArg::parse_serial(const std::string_view rest, bool flow_control_enabled
     return true;
 }
 
+bool CliArg::parse_raw(const std::string_view rest)
+{
+    // raw:// connection has no parameters
+    if (!rest.empty()) {
+        LogErr() << "raw:// connection should not have parameters";
+        return false;
+    }
+
+    protocol = Raw{};
+    return true;
+}
+
 } // namespace mavsdk

src/mavsdk/core/cli_arg.h

@@ -37,7 +37,11 @@ class CliArg {
         bool flow_control_enabled{false};
     };
 
-    using Protocol = std::variant<std::monostate, Udp, Tcp, Serial>;
+    struct Raw {
+        // No parameters needed for raw connection
+    };
+
+    using Protocol = std::variant<std::monostate, Udp, Tcp, Serial, Raw>;
 
     bool parse(const std::string& uri);
 
@@ -55,6 +59,7 @@ class CliArg {
     bool parse_tcpout(const std::string_view rest);
 
     bool parse_serial(const std::string_view rest, bool flow_control_enabled);
+    bool parse_raw(const std::string_view rest);
 };
 
 } // namespace mavsdk

src/mavsdk/core/cli_arg_test.cpp

@@ -381,3 +381,22 @@ TEST(CliArg, SerialWrong)
     EXPECT_FALSE(ca.parse("serial://COM3:-1"));
     EXPECT_FALSE(ca.parse("serial://COM3"));
 }
+
+TEST(CliArg, RawConnection)
+{
+    CliArg ca;
+
+    EXPECT_TRUE(ca.parse("raw://"));
+    auto raw = std::get_if<CliArg::Raw>(&ca.protocol);
+    ASSERT_TRUE(raw);
+}
+
+TEST(CliArg, RawConnectionWrong)
+{
+    CliArg ca;
+
+    // raw:// should not have any parameters
+    EXPECT_FALSE(ca.parse("raw://something"));
+    EXPECT_FALSE(ca.parse("raw://localhost"));
+    EXPECT_FALSE(ca.parse("raw://127.0.0.1"));
+}

src/mavsdk/core/include/mavsdk/mavsdk.h

@@ -463,6 +463,50 @@ class Mavsdk {
      */
     void intercept_outgoing_messages_async(std::function<bool(mavlink_message_t&)> callback);
 
+    /**
+     * @brief Callback type for raw bytes subscriptions.
+     */
+    using RawBytesCallback = std::function<void(const char* bytes, size_t length)>;
+
+    /**
+     * @brief Handle type for raw bytes subscriptions.
+     */
+    using RawBytesHandle = Handle<const char*, size_t>;
+
+    /**
+     * @brief Send raw MAVLink bytes.
+     *
+     * This allows sending raw MAVLink message bytes through all connections.
+     * The bytes can contain one or more MAVLink messages.
+     *
+     * @note When this API is first used, heartbeats will be automatically enabled.
+     *
+     * @param bytes Pointer to raw MAVLink message bytes.
+     * @param length Number of bytes to send.
+     * @return true if bytes were successfully sent, false otherwise.
+     */
+    bool send_raw_bytes(const char* bytes, size_t length);
+
+    /**
+     * @brief Subscribe to all outgoing raw bytes.
+     *
+     * This allows monitoring all MAVLink bytes that are sent out through any connection.
+     * The callback will be called with raw bytes after they are successfully sent.
+     *
+     * @note When this API is first used, heartbeats will be automatically enabled.
+     *
+     * @param callback Callback to be called with outgoing raw bytes.
+     * @return Handle to unsubscribe again.
+     */
+    RawBytesHandle subscribe_raw_bytes(RawBytesCallback callback);
+
+    /**
+     * @brief Unsubscribe from raw bytes.
+     *
+     * @param handle Handle from subscribe_raw_bytes.
+     */
+    void unsubscribe_raw_bytes(RawBytesHandle handle);
+
 private:
     static constexpr int DEFAULT_SYSTEM_ID_AUTOPILOT = 1;
     static constexpr int DEFAULT_COMPONENT_ID_AUTOPILOT = MAV_COMP_ID_AUTOPILOT1;

src/mavsdk/core/mavsdk.cpp

@@ -234,6 +234,21 @@ void Mavsdk::intercept_outgoing_messages_async(std::function<bool(mavlink_messag
     _impl->intercept_outgoing_messages_async(callback);
 }
 
+bool Mavsdk::send_raw_bytes(const char* bytes, size_t length)
+{
+    return _impl->send_raw_bytes(bytes, length);
+}
+
+Mavsdk::RawBytesHandle Mavsdk::subscribe_raw_bytes(RawBytesCallback callback)
+{
+    return _impl->subscribe_raw_bytes(callback);
+}
+
+void Mavsdk::unsubscribe_raw_bytes(RawBytesHandle handle)
+{
+    _impl->unsubscribe_raw_bytes(handle);
+}
+
 Mavsdk::ConnectionErrorHandle
 Mavsdk::subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback)
 {

src/mavsdk/core/mavsdk_impl.cpp

@@ -9,6 +9,7 @@
 #include "tcp_client_connection.h"
 #include "tcp_server_connection.h"
 #include "udp_connection.h"
+#include "raw_connection.h"
 #include "system.h"
 #include "system_impl.h"
 #include "serial_connection.h"
@@ -789,6 +790,9 @@ std::pair<ConnectionResult, Mavsdk::ConnectionHandle> MavsdkImpl::add_any_connec
             [this, forwarding_option](const CliArg::Serial& serial) {
                 return add_serial_connection(
                     serial.path, serial.baudrate, serial.flow_control_enabled, forwarding_option);
+            },
+            [this, forwarding_option](const CliArg::Raw&) {
+                return add_raw_connection(forwarding_option);
             }},
         cli_arg.protocol);
 }
@@ -929,6 +933,44 @@ std::pair<ConnectionResult, Mavsdk::ConnectionHandle> MavsdkImpl::add_serial_con
     }
 }
 
+std::pair<ConnectionResult, Mavsdk::ConnectionHandle>
+MavsdkImpl::add_raw_connection(ForwardingOption forwarding_option)
+{
+    // Check if a raw connection already exists
+    if (find_raw_connection() != nullptr) {
+        LogErr() << "Raw connection already exists. Only one raw connection is allowed.";
+        return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}};
+    }
+
+    auto new_conn = std::make_unique<RawConnection>(
+        [this](mavlink_message_t& message, Connection* connection) {
+            receive_message(message, connection);
+        },
+        [this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
+            receive_libmav_message(message, connection);
+        },
+        *this,
+        forwarding_option);
+
+    if (!new_conn) {
+        return {ConnectionResult::ConnectionError, Mavsdk::ConnectionHandle{}};
+    }
+
+    ConnectionResult ret = new_conn->start();
+    if (ret != ConnectionResult::Success) {
+        return {ret, Mavsdk::ConnectionHandle{}};
+    }
+
+    auto handle = add_connection(std::move(new_conn));
+
+    // Enable heartbeats for raw connection
+    auto new_configuration = get_configuration();
+    new_configuration.set_always_send_heartbeats(true);
+    set_configuration(new_configuration);
+
+    return {ConnectionResult::Success, handle};
+}
+
 Mavsdk::ConnectionHandle MavsdkImpl::add_connection(std::unique_ptr<Connection>&& new_connection)
 {
     std::lock_guard lock(_mutex);
@@ -1299,6 +1341,58 @@ void MavsdkImpl::unsubscribe_outgoing_messages_json(Mavsdk::InterceptJsonHandle
     }
 }
 
+RawConnection* MavsdkImpl::find_raw_connection()
+{
+    std::lock_guard lock(_mutex);
+
+    for (auto& entry : _connections) {
+        auto* raw_conn = dynamic_cast<RawConnection*>(entry.connection.get());
+        if (raw_conn != nullptr) {
+            return raw_conn;
+        }
+    }
+    return nullptr;
+}
+
+bool MavsdkImpl::send_raw_bytes(const char* bytes, size_t length)
+{
+    auto* raw_conn = find_raw_connection();
+    if (raw_conn == nullptr) {
+        LogErr()
+            << "No raw connection available. Please add one using add_any_connection(\"raw://\")";
+        return false;
+    }
+
+    raw_conn->receive(bytes, length);
+    return true;
+}
+
+Mavsdk::RawBytesHandle MavsdkImpl::subscribe_raw_bytes(const Mavsdk::RawBytesCallback& callback)
+{
+    if (find_raw_connection() == nullptr) {
+        LogWarn() << "No raw connection available. Subscription will only receive bytes after you "
+                     "add a connection using add_any_connection(\"raw://\")";
+    }
+    return _raw_bytes_subscriptions.subscribe(callback);
+}
+
+void MavsdkImpl::unsubscribe_raw_bytes(Mavsdk::RawBytesHandle handle)
+{
+    _raw_bytes_subscriptions.unsubscribe(handle);
+}
+
+bool MavsdkImpl::notify_raw_bytes_sent(const char* bytes, size_t length)
+{
+    if (_raw_bytes_subscriptions.empty()) {
+        return false;
+    }
+
+    _raw_bytes_subscriptions.queue(
+        bytes, length, [this](const std::function<void()>& func) { call_user_callback(func); });
+
+    return true;
+}
+
 Mavsdk::ConnectionErrorHandle
 MavsdkImpl::subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback)
 {

src/mavsdk/core/mavsdk_impl.h

@@ -41,6 +41,8 @@ class BufferParser;
 
 namespace mavsdk {
 
+class RawConnection; // Forward declaration
+
 class MavsdkImpl {
 public:
     MavsdkImpl(const Mavsdk::Configuration& configuration);
@@ -99,6 +101,12 @@ class MavsdkImpl {
     subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback);
     void unsubscribe_connection_errors(Mavsdk::ConnectionErrorHandle handle);
 
+    // Raw bytes API
+    bool send_raw_bytes(const char* bytes, size_t length);
+    Mavsdk::RawBytesHandle subscribe_raw_bytes(const Mavsdk::RawBytesCallback& callback);
+    void unsubscribe_raw_bytes(Mavsdk::RawBytesHandle handle);
+    bool notify_raw_bytes_sent(const char* bytes, size_t length);
+
     std::shared_ptr<ServerComponent> server_component(unsigned instance = 0);
 
     std::shared_ptr<ServerComponent>
@@ -151,6 +159,8 @@ class MavsdkImpl {
         int baudrate,
         bool flow_control,
         ForwardingOption forwarding_option);
+    std::pair<ConnectionResult, Mavsdk::ConnectionHandle>
+    add_raw_connection(ForwardingOption forwarding_option);
 
     Mavsdk::ConnectionHandle add_connection(std::unique_ptr<Connection>&& connection);
     void make_system_with_component(uint8_t system_id, uint8_t component_id);
@@ -183,6 +193,9 @@ class MavsdkImpl {
         std::vector<std::pair<Mavsdk::InterceptJsonHandle, Mavsdk::InterceptJsonCallback>>&
             callback_list);
 
+    // Find the raw connection in the connections list
+    RawConnection* find_raw_connection();
+
     mutable std::recursive_mutex _mutex{};
 
     // Message set for libmav message handling (shared across all connections)
@@ -244,6 +257,9 @@ class MavsdkImpl {
     mutable std::mutex _json_subscriptions_mutex{};
     HandleFactory<bool(Mavsdk::MavlinkMessage)> _json_handle_factory{};
 
+    // Raw bytes subscriptions
+    CallbackList<const char*, size_t> _raw_bytes_subscriptions{};
+
     std::atomic<double> _timeout_s{DEFAULT_TIMEOUT_S};
 
     struct ReceivedMessage {