Merge pull request #2682 from mavlink/pr-raw-bytes

core: add raw connection

Author: julianoes

docs/en/cpp/api_reference/classmavsdk_1_1_mavsdk.md

@@ -34,6 +34,8 @@ std::function< void()> [NewSystemCallback](#classmavsdk_1_1_mavsdk_1a7a283c6a75e
 [Handle](classmavsdk_1_1_handle.md)<> [NewSystemHandle](#classmavsdk_1_1_mavsdk_1ae0727f2bed9cbf276d161ada0a432b8c) | [Handle](classmavsdk_1_1_handle.md) type to unsubscribe from subscribe_on_new_system.
 [Handle](classmavsdk_1_1_handle.md)< bool([MavlinkMessage](structmavsdk_1_1_mavsdk_1_1_mavlink_message.md))> [InterceptJsonHandle](#classmavsdk_1_1_mavsdk_1a3b40ae4fd8af4c4419b61f0ad955812f) | [Handle](classmavsdk_1_1_handle.md) for intercepting messages.
 std::function< bool([MavlinkMessage](structmavsdk_1_1_mavsdk_1_1_mavlink_message.md))> [InterceptJsonCallback](#classmavsdk_1_1_mavsdk_1a17923db3b1504e911487729114b68f48) | Callback type for intercepting messages.
+std::function< void(const char *bytes, size_t length)> [RawBytesCallback](#classmavsdk_1_1_mavsdk_1acb5be9a1be97d251387ffe87ae8b9eb0) | Callback type for raw bytes subscriptions.
+[Handle](classmavsdk_1_1_handle.md)< const char *, size_t > [RawBytesHandle](#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1) | [Handle](classmavsdk_1_1_handle.md) type for raw bytes subscriptions.
 
 ## Public Member Functions
 
@@ -64,6 +66,9 @@ void | [unsubscribe_incoming_messages_json](#classmavsdk_1_1_mavsdk_1a4be244c389
 void | [unsubscribe_outgoing_messages_json](#classmavsdk_1_1_mavsdk_1aa3a490358db87cfed617cdad902bb753) ([InterceptJsonHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1a3b40ae4fd8af4c4419b61f0ad955812f) handle) | Unsubscribe from outgoing messages as JSON.
 void | [intercept_incoming_messages_async](#classmavsdk_1_1_mavsdk_1ac80c8909958533131cbdbc61d061794f) (std::function< bool(mavlink_message_t &)> callback) | Intercept incoming messages.
 void | [intercept_outgoing_messages_async](#classmavsdk_1_1_mavsdk_1a040ee5c1d41e71c0d63cf8f76d2db275) (std::function< bool(mavlink_message_t &)> callback) | Intercept outgoing messages.
+void | [pass_received_raw_bytes](#classmavsdk_1_1_mavsdk_1a65329315ac07bae110839d9e054fbc05) (const char * bytes, size_t length) | Pass received raw MAVLink bytes.
+[RawBytesHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1) | [subscribe_raw_bytes_to_be_sent](#classmavsdk_1_1_mavsdk_1a116e9bab0efdf7ec90866107ef517b20) ([RawBytesCallback](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1acb5be9a1be97d251387ffe87ae8b9eb0) callback) | Subscribe to raw bytes to be sent.
+void | [unsubscribe_raw_bytes_to_be_sent](#classmavsdk_1_1_mavsdk_1af6ec813a9728f4258056fa1f5d399eb1) ([RawBytesHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1) handle) | Unsubscribe from raw bytes to be sent.
 
 
 ## Constructor & Destructor Documentation
@@ -176,6 +181,26 @@ using mavsdk::Mavsdk::InterceptJsonCallback =  std::function<bool(MavlinkMessage
 Callback type for intercepting messages.
 
 
+### typedef RawBytesCallback {#classmavsdk_1_1_mavsdk_1acb5be9a1be97d251387ffe87ae8b9eb0}
+
+```cpp
+using mavsdk::Mavsdk::RawBytesCallback =  std::function<void(const char* bytes, size_t length)>
+```
+
+
+Callback type for raw bytes subscriptions.
+
+
+### typedef RawBytesHandle {#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1}
+
+```cpp
+using mavsdk::Mavsdk::RawBytesHandle =  Handle<const char*, size_t>
+```
+
+
+[Handle](classmavsdk_1_1_handle.md) type for raw bytes subscriptions.
+
+
 ## Member Function Documentation
 
 
@@ -584,4 +609,64 @@ This functionality is provided primarily for testing in order to simulate packet
 
 **Parameters**
 
-* std::function< bool(mavlink_message_t &)> **callback** - Callback to be called for each outgoing message. To drop a message, return 'false' from the callback.
+* std::function< bool(mavlink_message_t &)> **callback** - Callback to be called for each outgoing message. To drop a message, return 'false' from the callback.
+
+### pass_received_raw_bytes() {#classmavsdk_1_1_mavsdk_1a65329315ac07bae110839d9e054fbc05}
+```cpp
+void mavsdk::Mavsdk::pass_received_raw_bytes(const char *bytes, size_t length)
+```
+
+
+Pass received raw MAVLink bytes.
+
+This allows passing raw MAVLink message bytes into MAVSDK to be processed. The bytes can contain one or more MAVLink messages.
+
+
+::: info
+Before using this, run add_any_connection("raw://")
+:::
+
+This goes together with subscribe_raw_bytes_to_be_sent.
+
+**Parameters**
+
+* const char\* **bytes** - Pointer to raw MAVLink message bytes.
+* size_t **length** - Number of bytes to send.
+
+### subscribe_raw_bytes_to_be_sent() {#classmavsdk_1_1_mavsdk_1a116e9bab0efdf7ec90866107ef517b20}
+```cpp
+RawBytesHandle mavsdk::Mavsdk::subscribe_raw_bytes_to_be_sent(RawBytesCallback callback)
+```
+
+
+Subscribe to raw bytes to be sent.
+
+This allows getting MAVLink bytes that need to be sent out.
+
+
+::: info
+Before using this, run add_any_connection("raw://")
+:::
+
+This goes together with pass_received_raw_bytes. The bytes contain one mavlink message at a time.
+
+**Parameters**
+
+* [RawBytesCallback](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1acb5be9a1be97d251387ffe87ae8b9eb0) **callback** - Callback to be called with outgoing raw bytes.
+
+**Returns**
+
+&emsp;[RawBytesHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1) - [Handle](classmavsdk_1_1_handle.md) to unsubscribe again.
+
+### unsubscribe_raw_bytes_to_be_sent() {#classmavsdk_1_1_mavsdk_1af6ec813a9728f4258056fa1f5d399eb1}
+```cpp
+void mavsdk::Mavsdk::unsubscribe_raw_bytes_to_be_sent(RawBytesHandle handle)
+```
+
+
+Unsubscribe from raw bytes to be sent.
+
+
+**Parameters**
+
+* [RawBytesHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1ac766258f137aa3e8b0dabb5a66435ea1) **handle** - [Handle](classmavsdk_1_1_handle.md) from subscribe_raw_bytes_to_be_sent.

src/mavsdk/core/CMakeLists.txt

@@ -81,6 +81,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,53 @@ 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 Pass received raw MAVLink bytes.
+     *
+     * This allows passing raw MAVLink message bytes into MAVSDK to be processed.
+     * The bytes can contain one or more MAVLink messages.
+     *
+     * @note Before using this, run add_any_connection("raw://")
+     *
+     * This goes together with subscribe_raw_bytes_to_be_sent.
+     *
+     * @param bytes Pointer to raw MAVLink message bytes.
+     * @param length Number of bytes to send.
+     */
+    void pass_received_raw_bytes(const char* bytes, size_t length);
+
+    /**
+     * @brief Subscribe to raw bytes to be sent.
+     *
+     * This allows getting MAVLink bytes that need to be sent out.
+     *
+     * @note Before using this, run add_any_connection("raw://")
+     *
+     * This goes together with pass_received_raw_bytes.
+     * The bytes contain one mavlink message at a time.
+     *
+     * @param callback Callback to be called with outgoing raw bytes.
+     * @return Handle to unsubscribe again.
+     */
+    RawBytesHandle subscribe_raw_bytes_to_be_sent(RawBytesCallback callback);
+
+    /**
+     * @brief Unsubscribe from raw bytes to be sent.
+     *
+     * @param handle Handle from subscribe_raw_bytes_to_be_sent.
+     */
+    void unsubscribe_raw_bytes_to_be_sent(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);
 }
 
+void Mavsdk::pass_received_raw_bytes(const char* bytes, size_t length)
+{
+    _impl->pass_received_raw_bytes(bytes, length);
+}
+
+Mavsdk::RawBytesHandle Mavsdk::subscribe_raw_bytes_to_be_sent(RawBytesCallback callback)
+{
+    return _impl->subscribe_raw_bytes_to_be_sent(callback);
+}
+
+void Mavsdk::unsubscribe_raw_bytes_to_be_sent(RawBytesHandle handle)
+{
+    _impl->unsubscribe_raw_bytes_to_be_sent(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"
@@ -790,6 +791,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);
 }
@@ -930,6 +934,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);
@@ -1318,6 +1360,57 @@ 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;
+}
+
+void MavsdkImpl::pass_received_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;
+    }
+
+    raw_conn->receive(bytes, length);
+}
+
+Mavsdk::RawBytesHandle
+MavsdkImpl::subscribe_raw_bytes_to_be_sent(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_to_be_sent(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(bytes, length);
+
+    return true;
+}
+
 Mavsdk::ConnectionErrorHandle
 MavsdkImpl::subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback)
 {