diff --git a/cpp/docs/en/cpp/api_reference/classmavsdk_1_1_mavsdk.md b/cpp/docs/en/cpp/api_reference/classmavsdk_1_1_mavsdk.md index 30a211a77a..9c69c45f87 100644 --- a/cpp/docs/en/cpp/api_reference/classmavsdk_1_1_mavsdk.md +++ b/cpp/docs/en/cpp/api_reference/classmavsdk_1_1_mavsdk.md @@ -68,6 +68,8 @@ void | [unsubscribe_incoming_messages_json](#classmavsdk_1_1_mavsdk_1a4be244c389 [InterceptJsonHandle](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1a3b40ae4fd8af4c4419b61f0ad955812f) | [subscribe_outgoing_messages_json](#classmavsdk_1_1_mavsdk_1a58f85b2f74a32404a8e975feefed8f47) (const [InterceptJsonCallback](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1a17923db3b1504e911487729114b68f48) & callback) | Intercept outgoing messages as JSON. 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. +bool | [start_tlog_recording](#classmavsdk_1_1_mavsdk_1af8ed201951f4b264a864217c5e5db5c1) (const std::string & path) | Start recording all incoming MAVLink traffic to a .tlog file. +void | [stop_tlog_recording](#classmavsdk_1_1_mavsdk_1a507d9f58439233b5ddd3d5d1ba30bc0c) () | Stop recording and close the .tlog file. 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. @@ -110,7 +112,7 @@ mavsdk::Mavsdk::~Mavsdk() Destructor. -Disconnects all connected vehicles and releases all resources. +Disconnects all connected vehicles and releases all resources. Any active .tlog recording is automatically stopped and flushed. ## Member Typdef Documentation @@ -647,6 +649,40 @@ This functionality is provided primarily for testing in order to simulate packet * std::function< bool(mavlink_message_t &)> **callback** - Callback to be called for each incoming message. To drop a message, return 'false' from the callback. +### start_tlog_recording() {#classmavsdk_1_1_mavsdk_1af8ed201951f4b264a864217c5e5db5c1} +```cpp +bool mavsdk::Mavsdk::start_tlog_recording(const std::string &path) +``` + + +Start recording all incoming MAVLink traffic to a .tlog file. + +A .tlog (telemetry log) is a binary file where each record consists of an 8-byte big-endian microsecond Unix timestamp followed by the raw MAVLink wire packet. The format is compatible with [Mission](classmavsdk_1_1_mission.md) Planner, MAVProxy, and pymavlink. + + +Recording captures traffic across the entire [Mavsdk](classmavsdk_1_1_mavsdk.md) instance (all connected systems and connections), not per-system. If recording is already active it is stopped and restarted with the new file. + + +The recording is automatically stopped and flushed when the [Mavsdk](classmavsdk_1_1_mavsdk.md) instance is destroyed, so explicit [stop_tlog_recording()](classmavsdk_1_1_mavsdk.md#classmavsdk_1_1_mavsdk_1a507d9f58439233b5ddd3d5d1ba30bc0c) is optional. + +**Parameters** + +* const std::string& **path** - Output file path (e.g. "flight.tlog"). + +**Returns** + + bool - true if the file was opened successfully, false otherwise. + +### stop_tlog_recording() {#classmavsdk_1_1_mavsdk_1a507d9f58439233b5ddd3d5d1ba30bc0c} +```cpp +void mavsdk::Mavsdk::stop_tlog_recording() +``` + + +Stop recording and close the .tlog file. + +Does nothing if recording is not active. Automatically called on [Mavsdk](classmavsdk_1_1_mavsdk.md) destruction. + ### intercept_outgoing_messages_async() {#classmavsdk_1_1_mavsdk_1a040ee5c1d41e71c0d63cf8f76d2db275} ```cpp void mavsdk::Mavsdk::intercept_outgoing_messages_async(std::function< bool(mavlink_message_t &)> callback) diff --git a/cpp/docs/en/cpp/api_reference/structmavsdk_1_1_mavsdk_1_1_mavlink_message.md b/cpp/docs/en/cpp/api_reference/structmavsdk_1_1_mavsdk_1_1_mavlink_message.md index 24713db4f1..c05422c65c 100644 --- a/cpp/docs/en/cpp/api_reference/structmavsdk_1_1_mavsdk_1_1_mavlink_message.md +++ b/cpp/docs/en/cpp/api_reference/structmavsdk_1_1_mavsdk_1_1_mavlink_message.md @@ -22,6 +22,8 @@ uint32_t [target_component_id](#structmavsdk_1_1_mavsdk_1_1_mavlink_message_1ad1 std::string [fields_json](#structmavsdk_1_1_mavsdk_1_1_mavlink_message_1a73165621a97083c143ba7c36461c9754) {} - All message fields as single JSON object. +std::vector< uint8_t > [raw_bytes](#structmavsdk_1_1_mavsdk_1_1_mavlink_message_1aa4fc863b9e0103a894fdd99d5219847b) {} - Raw MAVLink wire bytes (populated for incoming messages; empty when sending) + ## Field Documentation @@ -85,3 +87,13 @@ std::string mavsdk::Mavsdk::MavlinkMessage::fields_json {} All message fields as single JSON object. + +### raw_bytes {#structmavsdk_1_1_mavsdk_1_1_mavlink_message_1aa4fc863b9e0103a894fdd99d5219847b} + +```cpp +std::vector mavsdk::Mavsdk::MavlinkMessage::raw_bytes {} +``` + + +Raw MAVLink wire bytes (populated for incoming messages; empty when sending) + diff --git a/cpp/examples/CMakeLists.txt b/cpp/examples/CMakeLists.txt index 7ac8b454fc..26f9f8bcdc 100644 --- a/cpp/examples/CMakeLists.txt +++ b/cpp/examples/CMakeLists.txt @@ -61,6 +61,7 @@ add_subdirectory(tune) add_subdirectory(vtol_transition) add_subdirectory(winch) add_subdirectory(wind) +add_subdirectory(write_tlog) # Disabled, requires mavsdk_server library. #add_subdirectory(start_stop_server) diff --git a/cpp/examples/write_tlog/CMakeLists.txt b/cpp/examples/write_tlog/CMakeLists.txt new file mode 100644 index 0000000000..04e78a5c29 --- /dev/null +++ b/cpp/examples/write_tlog/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.10.2) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +project(write_tlog) + +add_executable(write_tlog + write_tlog.cpp +) + +find_package(MAVSDK REQUIRED) + +target_link_libraries(write_tlog + MAVSDK::mavsdk +) + +if(NOT MSVC) + add_compile_options(write_tlog PRIVATE -Wall -Wextra) +else() + add_compile_options(write_tlog PRIVATE -W2) +endif() diff --git a/cpp/examples/write_tlog/write_tlog.cpp b/cpp/examples/write_tlog/write_tlog.cpp new file mode 100644 index 0000000000..6d24c9fe34 --- /dev/null +++ b/cpp/examples/write_tlog/write_tlog.cpp @@ -0,0 +1,103 @@ +// +// Example: record all incoming MAVLink traffic to a TLOG file. +// +// Usage: +// write_tlog +// +// Example (SITL): +// write_tlog udpin://0.0.0.0:14540 flight.tlog +// +// Press Ctrl+C to stop recording. The file can be opened in QGroundControl. +// + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace mavsdk; + +static std::atomic should_exit{false}; + +static void signal_handler(int /*signum*/) +{ + should_exit.store(true, std::memory_order_relaxed); +} + +static void usage(const char* bin) +{ + std::cerr << "Usage: " << bin << " \n" + << "Connection URL examples:\n" + << " UDP server : udpin://0.0.0.0:14540\n" + << " UDP client : udpout://127.0.0.1:14540\n" + << " TCP client : tcpout://192.168.1.1:5760\n" + << " Serial : serial:///dev/ttyUSB0:57600\n"; +} + +int main(int argc, char** argv) +{ + if (argc != 3) { + usage(argv[0]); + return 1; + } + + const std::string connection_url = argv[1]; + const std::string tlog_path = argv[2]; + + // Register signal handler so Ctrl+C flushes and closes the file cleanly. + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::CompanionComputer}}; + + const ConnectionResult connection_result = mavsdk.add_any_connection(connection_url); + if (connection_result != ConnectionResult::Success) { + std::cerr << "Connection failed: " << connection_result << '\n'; + return 1; + } + + // Start recording immediately so the initial handshake messages are captured too. + if (!mavsdk.start_tlog_recording(tlog_path)) { + std::cerr << "Failed to open '" << tlog_path << "' for writing.\n"; + return 1; + } + + std::cout << "Recording to '" << tlog_path << "'. Waiting to discover system...\n"; + + auto prom = std::promise>{}; + auto fut = prom.get_future(); + bool promise_set = false; + + Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&]() { + auto system = mavsdk.systems().back(); + if (!promise_set) { + promise_set = true; + prom.set_value(system); + } + }); + + if (fut.wait_for(std::chrono::seconds(5)) == std::future_status::timeout) { + std::cerr << "No system found within 5 s. Is the vehicle connected?\n"; + mavsdk.stop_tlog_recording(); + return 1; + } + + mavsdk.unsubscribe_on_new_system(handle); + auto system = fut.get(); + std::cout << "Discovered system (sysid " << static_cast(system->get_system_id()) + << "). Press Ctrl+C to stop...\n"; + + while (!should_exit.load(std::memory_order_relaxed)) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + + std::cout << "\nStopping recording.\n"; + mavsdk.stop_tlog_recording(); + + return 0; +} diff --git a/cpp/src/mavsdk/core/include/mavsdk/mavsdk.hpp b/cpp/src/mavsdk/core/include/mavsdk/mavsdk.hpp index eebe90c933..b89d028ca7 100644 --- a/cpp/src/mavsdk/core/include/mavsdk/mavsdk.hpp +++ b/cpp/src/mavsdk/core/include/mavsdk/mavsdk.hpp @@ -332,6 +332,7 @@ class MAVSDK_PUBLIC Mavsdk { * @brief Destructor. * * Disconnects all connected vehicles and releases all resources. + * Any active .tlog recording is automatically stopped and flushed. */ ~Mavsdk(); @@ -472,6 +473,8 @@ class MAVSDK_PUBLIC Mavsdk { uint32_t target_component_id{}; /**< @brief Target component ID (for sending, 0 for broadcast) */ std::string fields_json{}; /**< @brief All message fields as single JSON object */ + std::vector raw_bytes{}; /**< @brief Raw MAVLink wire bytes (populated for + incoming messages; empty when sending) */ }; /** @@ -530,6 +533,34 @@ class MAVSDK_PUBLIC Mavsdk { */ void intercept_incoming_messages_async(std::function callback); + /** + * @brief Start recording all incoming MAVLink traffic to a .tlog file. + * + * A .tlog (telemetry log) is a binary file where each record consists of + * an 8-byte big-endian microsecond Unix timestamp followed by the raw + * MAVLink wire packet. The format is compatible with + * Mission Planner, MAVProxy, and pymavlink. + * + * Recording captures traffic across the entire Mavsdk instance (all + * connected systems and connections), not per-system. If recording is + * already active it is stopped and restarted with the new file. + * + * The recording is automatically stopped and flushed when the Mavsdk + * instance is destroyed, so explicit stop_tlog_recording() is optional. + * + * @param path Output file path (e.g. "flight.tlog"). + * @return true if the file was opened successfully, false otherwise. + */ + [[nodiscard]] bool start_tlog_recording(const std::string& path); + + /** + * @brief Stop recording and close the .tlog file. + * + * Does nothing if recording is not active. Automatically called on + * Mavsdk destruction. + */ + void stop_tlog_recording(); + /** * @brief Intercept outgoing messages. * diff --git a/cpp/src/mavsdk/core/libmav_receiver.cpp b/cpp/src/mavsdk/core/libmav_receiver.cpp index 23192b52e4..25cd793b8a 100644 --- a/cpp/src/mavsdk/core/libmav_receiver.cpp +++ b/cpp/src/mavsdk/core/libmav_receiver.cpp @@ -89,6 +89,18 @@ bool LibmavReceiver::parse_libmav_message_from_buffer() _last_message.system_id = header.systemId(); _last_message.component_id = header.componentId(); + if (_last_libmav_message) { + const uint32_t wire_len = _last_libmav_message->finalizedSize(); + if (wire_len > 0) { + const uint8_t* wire_ptr = _last_libmav_message->data(); + _last_message.raw_bytes.assign(wire_ptr, wire_ptr + wire_len); + } else { + _last_message.raw_bytes.clear(); + } + } else { + _last_message.raw_bytes.clear(); + } + // Extract target_system and target_component if present in message fields uint8_t target_system_id = 0; uint8_t target_component_id = 0; diff --git a/cpp/src/mavsdk/core/mavsdk.cpp b/cpp/src/mavsdk/core/mavsdk.cpp index 29ab4eb54c..00c74c255c 100644 --- a/cpp/src/mavsdk/core/mavsdk.cpp +++ b/cpp/src/mavsdk/core/mavsdk.cpp @@ -248,6 +248,16 @@ void Mavsdk::intercept_outgoing_messages_async(std::functionintercept_outgoing_messages_async(callback); } +bool Mavsdk::start_tlog_recording(const std::string& path) +{ + return _impl->start_tlog_recording(path); +} + +void Mavsdk::stop_tlog_recording() +{ + _impl->stop_tlog_recording(); +} + void Mavsdk::pass_received_raw_bytes(const char* bytes, size_t length) { _impl->pass_received_raw_bytes(bytes, length); diff --git a/cpp/src/mavsdk/core/mavsdk_impl.cpp b/cpp/src/mavsdk/core/mavsdk_impl.cpp index deeeafe1ee..5e76cf474d 100644 --- a/cpp/src/mavsdk/core/mavsdk_impl.cpp +++ b/cpp/src/mavsdk/core/mavsdk_impl.cpp @@ -3,6 +3,9 @@ #include #include +#include +#include +#include #include #include #include "connection.hpp" @@ -17,16 +20,31 @@ #include "version.hpp" #include "server_component_impl.hpp" #include "overloaded.hpp" +#include "mavlink_address.hpp" #include "mavlink_channels.hpp" +#include "mavlink_command_receiver.hpp" #include "callback_list.tpp" #include "hostname_to_ip.hpp" +#include "libmav_receiver.hpp" #include "embedded_mavlink_xml.hpp" +#include #include #include "mavsdk_export.h" namespace mavsdk { +struct TlogFile { + std::ofstream stream; + ~TlogFile() + { + if (stream.is_open()) { + stream.flush(); + stream.close(); + } + } +}; + template class MAVSDK_TEMPL_INST CallbackList<>; MavsdkImpl::MavsdkImpl(const Mavsdk::Configuration& configuration) : @@ -106,6 +124,9 @@ MavsdkImpl::~MavsdkImpl() _io_thread.reset(); } + // Flush and close any open tlog file now that no more messages can arrive. + stop_tlog_recording(); + if (_process_user_callbacks_thread) { _user_callback_queue.stop(); _process_user_callbacks_thread->join(); @@ -436,6 +457,38 @@ void MavsdkImpl::process_message(mavlink_message_t& message, Connection* connect return; } + // Record to tlog before any intercept/drop logic so all received traffic is captured. + { + std::lock_guard tlog_lock(_tlog_mutex); + if (_tlog_file && _tlog_file->stream.is_open()) { + constexpr size_t timestamp_bytes = 8; + // Big-endian microsecond Unix timestamp. + const auto now_us = + static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + std::array ts{}; + uint64_t v = now_us; + for (int i = static_cast(timestamp_bytes) - 1; i >= 0; --i) { + ts.at(static_cast(i)) = static_cast(v & 0xFFu); + v >>= 8u; + } + _tlog_file->stream.write( + reinterpret_cast(ts.data()), + static_cast(timestamp_bytes)); + + std::array wire{}; + const uint16_t wire_len = mavlink_msg_to_send_buffer(wire.data(), &message); + _tlog_file->stream.write(reinterpret_cast(wire.data()), wire_len); + _tlog_file->stream.flush(); + + if (!_tlog_file->stream.good()) { + LogErr("tlog: write failed, stopping recording"); + _tlog_file.reset(); + } + } + } + { std::lock_guard lock(_mutex); @@ -588,6 +641,37 @@ void MavsdkImpl::process_libmav_message( return; } + // Record to tlog before any processing so all received traffic is captured. + if (!message.raw_bytes.empty()) { + std::lock_guard tlog_lock(_tlog_mutex); + if (_tlog_file && _tlog_file->stream.is_open()) { + constexpr size_t timestamp_bytes = 8; + // Big-endian microsecond Unix timestamp. + const auto now_us = + static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + std::array ts{}; + uint64_t v = now_us; + for (int i = static_cast(timestamp_bytes) - 1; i >= 0; --i) { + ts.at(static_cast(i)) = static_cast(v & 0xFFu); + v >>= 8u; + } + _tlog_file->stream.write( + reinterpret_cast(ts.data()), + static_cast(timestamp_bytes)); + _tlog_file->stream.write( + reinterpret_cast(message.raw_bytes.data()), + static_cast(message.raw_bytes.size())); + _tlog_file->stream.flush(); + + if (!_tlog_file->stream.good()) { + LogErr("tlog: write failed, stopping recording"); + _tlog_file.reset(); + } + } + } + { std::lock_guard lock(_mutex); @@ -1437,6 +1521,28 @@ void MavsdkImpl::intercept_outgoing_messages_async(std::function lock(_tlog_mutex); + if (_tlog_file && _tlog_file->stream.is_open()) { + _tlog_file->stream.flush(); + _tlog_file->stream.close(); + } + _tlog_file = std::make_unique(); + _tlog_file->stream.open(path, std::ios::binary | std::ios::trunc); + return _tlog_file->stream.is_open(); +} + +void MavsdkImpl::stop_tlog_recording() +{ + std::lock_guard lock(_tlog_mutex); + if (_tlog_file && _tlog_file->stream.is_open()) { + _tlog_file->stream.flush(); + _tlog_file->stream.close(); + } + _tlog_file.reset(); +} + bool MavsdkImpl::call_json_interception_callbacks( const Mavsdk::MavlinkMessage& json_message, std::vector>& diff --git a/cpp/src/mavsdk/core/mavsdk_impl.hpp b/cpp/src/mavsdk/core/mavsdk_impl.hpp index e0f53b88d8..dcf2f14805 100644 --- a/cpp/src/mavsdk/core/mavsdk_impl.hpp +++ b/cpp/src/mavsdk/core/mavsdk_impl.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include @@ -17,16 +16,12 @@ #include "call_every_handler.hpp" #include "component_type.hpp" #include "connection.hpp" -#include "libmav_receiver.hpp" -#include #include "cli_arg.hpp" #include "handle_factory.hpp" #include "handle.hpp" #include "mavsdk.hpp" #include "mavlink_include.hpp" -#include "mavlink_address.hpp" #include "mavlink_message_handler.hpp" -#include "mavlink_command_receiver.hpp" #include "locked_queue.hpp" #include "server_component.hpp" #include "system.hpp" @@ -35,7 +30,6 @@ #include "callback_list.hpp" #include "callback_tracker.hpp" -// Forward declarations to avoid including MessageSet.h in header namespace mav { class MessageSet; class Message; @@ -45,7 +39,9 @@ class BufferParser; namespace mavsdk { -class RawConnection; // Forward declaration +class RawConnection; + +struct TlogFile; class MavsdkImpl { public: @@ -107,6 +103,9 @@ class MavsdkImpl { void intercept_incoming_messages_async(std::function callback); void intercept_outgoing_messages_async(std::function callback); + bool start_tlog_recording(const std::string& path); + void stop_tlog_recording(); + // JSON message interception Mavsdk::InterceptJsonHandle subscribe_incoming_messages_json(const Mavsdk::InterceptJsonCallback& callback); @@ -271,6 +270,9 @@ class MavsdkImpl { std::function _intercept_incoming_messages_callback{nullptr}; std::function _intercept_outgoing_messages_callback{nullptr}; + std::mutex _tlog_mutex{}; + std::unique_ptr _tlog_file{}; + // JSON message interception std::vector> _incoming_json_message_subscriptions{}; diff --git a/cpp/src/mavsdk/plugins/mavlink_direct/mavlink_direct_impl.cpp b/cpp/src/mavsdk/plugins/mavlink_direct/mavlink_direct_impl.cpp index 80de2ab7fa..2d07e05f48 100644 --- a/cpp/src/mavsdk/plugins/mavlink_direct/mavlink_direct_impl.cpp +++ b/cpp/src/mavsdk/plugins/mavlink_direct/mavlink_direct_impl.cpp @@ -61,11 +61,6 @@ void MavlinkDirectImpl::init() void MavlinkDirectImpl::deinit() { - // Synchronise with any in-flight I/O-thread dispatch: _callbacks()/exec() holds - // the CallbackList's internal mutex, and clear() acquires the same mutex, so - // clear() blocks until the current exec() finishes. After clear() the list is - // empty; subsequent exec() calls are no-ops, making it safe to destroy the - // CallbackList. This also prevents any further user callbacks from being enqueued. _callbacks.clear(); if (_system_subscription.valid()) { @@ -158,11 +153,6 @@ MavlinkDirect::Result MavlinkDirectImpl::send_message(MavlinkDirect::MavlinkMess MavlinkDirect::MessageHandle MavlinkDirectImpl::subscribe_message( std::string message_name, const MavlinkDirect::MessageCallback& callback) { - // filtering_callback is called synchronously on the I/O thread (via _callbacks()/exec() - // in init()). It captures 'this' only to reach call_user_callback, and is never - // enqueued anywhere — so there is no lifetime issue. deinit() calls _callbacks.clear() - // before unregistering the system handler, which blocks until any in-flight exec() - // completes; after that, filtering_callback will never be called again. auto filtering_callback = [this, message_name, callback](const MavlinkDirect::MavlinkMessage& message) { if (!message_name.empty() && message_name != message.message_name) {