diff --git a/examples/ulog_data.cpp b/examples/ulog_data.cpp new file mode 100644 index 0000000..51ebbdc --- /dev/null +++ b/examples/ulog_data.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include "data_container.hpp" +#include "reader.hpp" + +int main(int argc, char** argv) +{ + if (argc < 2) { + printf("Usage: %s \n", argv[0]); + return -1; + } + FILE* file = fopen(argv[1], "rb"); + if (!file) { + printf("opening file failed\n"); + return -1; + } + uint8_t buffer[4048]; + int bytes_read; + const auto data_container = + std::make_shared(ulog_cpp::DataContainer::StorageConfig::FullLog); + ulog_cpp::Reader reader{data_container}; + while ((bytes_read = fread(buffer, 1, sizeof(buffer), file)) > 0) { + reader.readChunk(buffer, bytes_read); + } + fclose(file); + + // Check for errors + if (!data_container->parsingErrors().empty()) { + printf("###### File Parsing Errors ######\n"); + for (const auto& parsing_error : data_container->parsingErrors()) { + printf(" %s\n", parsing_error.c_str()); + } + } + if (data_container->hadFatalError()) { + printf("Fatal parsing error, exiting\n"); + return -1; + } + + // Read out some data + // TODO: create a simpler API for this + const std::string message = "multirotor_motor_limits"; + printf("%s timestamps: ", message.c_str()); + for (const auto& sub : data_container->subscriptions()) { + if (sub.second.add_logged_message.messageName() == message) { + const auto& fields = data_container->messageFormats().at(message).fields(); + // Expect the first field to be the timestamp + if (fields[0].name != "timestamp") { + printf("Error: first field is not 'timestamp'\n"); + return -1; + } + for (const auto& data : sub.second.data) { + auto value = ulog_cpp::Value( + fields[0], + std::vector(data.data().begin(), data.data().begin() + sizeof(uint64_t))); + printf("%lu, ", std::get(value.data())); + } + } + } + printf("\n"); + + return 0; +} diff --git a/examples/ulog_info.cpp b/examples/ulog_info.cpp new file mode 100644 index 0000000..1748c56 --- /dev/null +++ b/examples/ulog_info.cpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include "data_container.hpp" +#include "reader.hpp" + +int main(int argc, char** argv) +{ + if (argc < 2) { + printf("Usage: %s \n", argv[0]); + return -1; + } + FILE* file = fopen(argv[1], "rb"); + if (!file) { + printf("opening file failed\n"); + return -1; + } + uint8_t buffer[4048]; + int bytes_read; + const auto data_container = + std::make_shared(ulog_cpp::DataContainer::StorageConfig::FullLog); + ulog_cpp::Reader reader{data_container}; + while ((bytes_read = fread(buffer, 1, sizeof(buffer), file)) > 0) { + reader.readChunk(buffer, bytes_read); + } + fclose(file); + + // Check for errors + if (!data_container->parsingErrors().empty()) { + printf("###### File Parsing Errors ######\n"); + for (const auto& parsing_error : data_container->parsingErrors()) { + printf(" %s\n", parsing_error.c_str()); + } + } + if (data_container->hadFatalError()) { + printf("Fatal parsing error, exiting\n"); + return -1; + } + + // Print info + // Dropouts + const auto& dropouts = data_container->dropouts(); + const int total_dropouts_ms = std::accumulate( + dropouts.begin(), dropouts.end(), 0, + [](int sum, const ulog_cpp::Dropout& curr) { return sum + curr.durationMs(); }); + printf("Dropouts: %zu, total duration: %i ms\n", dropouts.size(), total_dropouts_ms); + + auto print_value = [](const std::string& name, const ulog_cpp::Value& value) { + if (const auto* const str_ptr(std::get_if(&value.data())); str_ptr) { + printf(" %s: %s\n", name.c_str(), str_ptr->c_str()); + } else if (const auto* const int_ptr(std::get_if(&value.data())); int_ptr) { + printf(" %s: %i\n", name.c_str(), *int_ptr); + } else if (const auto* const uint_ptr(std::get_if(&value.data())); uint_ptr) { + printf(" %s: %u\n", name.c_str(), *uint_ptr); + } else if (const auto* const float_ptr(std::get_if(&value.data())); float_ptr) { + printf(" %s: %.3f\n", name.c_str(), static_cast(*float_ptr)); + } else { + printf(" %s: \n", name.c_str()); + } + }; + + // Info messages + printf("Info Messages:\n"); + for (const auto& info_msg : data_container->messageInfo()) { + print_value(info_msg.second.field().name, info_msg.second.value()); + } + // Info multi messages + printf("Info Multiple Messages:"); + for (const auto& info_msg : data_container->messageInfoMulti()) { + printf(" [%s: %zu],", info_msg.first.c_str(), info_msg.second.size()); + } + printf("\n"); + + // Messages + printf("\n"); + printf("Name (multi id) - number of data points\n"); + + // Sort by name & multi id + const auto& subscriptions = data_container->subscriptions(); + std::vector sorted_subscription_ids(subscriptions.size()); + std::transform(subscriptions.begin(), subscriptions.end(), sorted_subscription_ids.begin(), + [](const auto& pair) { return pair.first; }); + std::sort(sorted_subscription_ids.begin(), sorted_subscription_ids.end(), + [&subscriptions](const uint16_t a, const uint16_t b) { + const auto& add_logged_a = subscriptions.at(a).add_logged_message; + const auto& add_logged_b = subscriptions.at(b).add_logged_message; + if (add_logged_a.messageName() == add_logged_b.messageName()) { + return add_logged_a.multiId() < add_logged_b.multiId(); + } + return add_logged_a.messageName() < add_logged_b.messageName(); + }); + for (const auto& subscription_id : sorted_subscription_ids) { + const auto& subscription = subscriptions.at(subscription_id); + const int multi_instance = subscription.add_logged_message.multiId(); + const std::string message_name = subscription.add_logged_message.messageName(); + printf(" %s (%i) - %zu\n", message_name.c_str(), multi_instance, subscription.data.size()); + } + + printf("Formats:\n"); + for (const auto& msg_format : data_container->messageFormats()) { + std::string format_fields; + for (const auto& field : msg_format.second.fields()) { + format_fields += field.encode() + ", "; + } + printf(" %s: %s\n", msg_format.second.name().c_str(), format_fields.c_str()); + } + + // logging + printf("Logging:\n"); + for (const auto& logging : data_container->logging()) { + std::string tag_str; + if (logging.hasTag()) { + tag_str = std::to_string(logging.tag()) + " "; + } + printf(" %s<%s> %lu %s\n", tag_str.c_str(), logging.logLevelStr().c_str(), logging.timestamp(), + logging.message().c_str()); + } + + // Params (init, after, defaults) + printf("Default Params:\n"); + for (const auto& default_param : data_container->defaultParameters()) { + print_value(default_param.second.field().name, default_param.second.value()); + } + printf("Initial Params:\n"); + for (const auto& default_param : data_container->initialParameters()) { + print_value(default_param.second.field().name, default_param.second.value()); + } + + return 0; +} diff --git a/examples/ulog_writer.cpp b/examples/ulog_writer.cpp new file mode 100644 index 0000000..e93f29f --- /dev/null +++ b/examples/ulog_writer.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include "simple_writer.hpp" + +using namespace std::chrono_literals; + +static uint64_t currentTimeUs() +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +struct MyData { + uint64_t timestamp; + float debug_array[4]; + float cpuload; + float temperature; + int8_t counter; + + static std::string messageName() { return "my_data"; } + + static std::vector fields() + { + // clang-format off + return { + {"uint64_t", "timestamp"}, // Monotonic timestamp in microseconds (since boot), must always be the first field + {"float", "debug_array", 4}, + {"float", "cpuload"}, + {"float", "temperature"}, + {"int8_t", "counter"}, + }; // clang-format on + } +}; + +int main(int argc, char** argv) +{ + if (argc < 2) { + printf("Usage: %s \n", argv[0]); + return -1; + } + + try { + ulog_cpp::SimpleWriter writer(argv[1], currentTimeUs()); + // See https://docs.px4.io/main/en/dev_log/ulog_file_format.html#i-information-message for + // well-known keys + writer.writeInfo("sys_name", "ULogExampleWriter"); + + writer.writeParameter("PARAM_A", 382.23F); + writer.writeParameter("PARAM_B", 8272); + + writer.writeMessageFormat(MyData::messageName(), MyData::fields()); + writer.headerComplete(); + + const uint16_t my_data_msg_id = writer.writeAddLoggedMessage(MyData::messageName()); + + writer.writeTextMessage(ulog_cpp::Logging::Level::Info, "Hello world", currentTimeUs()); + + float cpuload = 25.423F; + for (int i = 0; i < 100; ++i) { + MyData data{}; + data.timestamp = currentTimeUs(); + data.cpuload = cpuload; + data.counter = i; + writer.writeData(my_data_msg_id, data); + cpuload -= 0.424F; + + std::this_thread::sleep_for(10ms); + } + } catch (const ulog_cpp::ExceptionBase& e) { + printf("ULog exception: %s\n", e.what()); + } + + return 0; +} diff --git a/ulog_cpp/CMakeLists.txt b/ulog_cpp/CMakeLists.txt new file mode 100644 index 0000000..6aa50e7 --- /dev/null +++ b/ulog_cpp/CMakeLists.txt @@ -0,0 +1,23 @@ + +add_library(ulog_cpp + data_container.cpp + messages.cpp + reader.cpp + writer.cpp + simple_writer.cpp +) + +add_executable(ulog_info ulog_info.cpp) +target_link_libraries(ulog_info PUBLIC + ulog_cpp + ) + +add_executable(ulog_data ulog_data.cpp) +target_link_libraries(ulog_data PUBLIC + ulog_cpp + ) + +add_executable(ulog_writer ulog_writer.cpp) +target_link_libraries(ulog_writer PUBLIC + ulog_cpp + ) diff --git a/ulog_cpp/data_container.cpp b/ulog_cpp/data_container.cpp new file mode 100644 index 0000000..86bc400 --- /dev/null +++ b/ulog_cpp/data_container.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "data_container.hpp" + +namespace ulog_cpp { + +DataContainer::DataContainer(DataContainer::StorageConfig storage_config) + : _storage_config(storage_config) +{ +} +void DataContainer::error(const std::string& msg, bool is_recoverable) +{ + if (!is_recoverable) { + _had_fatal_error = true; + } + _parsing_errors.push_back(msg); +} + +void DataContainer::headerComplete() +{ + _header_complete = true; +} +void DataContainer::fileHeader(const FileHeader& header) +{ + _file_header = header; +} +void DataContainer::messageInfo(const MessageInfo& message_info) +{ + if (_header_complete && _storage_config == StorageConfig::Header) { + return; + } + if (message_info.isMulti()) { + if (message_info.isContinued()) { + auto& messages = _message_info_multi[message_info.field().name]; + if (messages.empty()) { + throw ParsingException("info_multi msg is continued, but no previous"); + } + messages[messages.size() - 1].push_back(message_info); + } else { + _message_info_multi[message_info.field().name].push_back({message_info}); + } + } else { + _message_info.insert({message_info.field().name, message_info}); + } +} +void DataContainer::messageFormat(const MessageFormat& message_format) +{ + if (_message_formats.find(message_format.name()) != _message_formats.end()) { + throw ParsingException("Duplicate message format"); + } + _message_formats.insert({message_format.name(), message_format}); +} +void DataContainer::parameter(const Parameter& parameter) +{ + if (_header_complete && _storage_config == StorageConfig::Header) { + return; + } + if (_header_complete) { + _changed_parameters.push_back(parameter); + } else { + _initial_parameters.insert({parameter.field().name, parameter}); + } +} +void DataContainer::parameterDefault(const ParameterDefault& parameter_default) +{ + _default_parameters.insert({parameter_default.field().name, parameter_default}); +} +void DataContainer::addLoggedMessage(const AddLoggedMessage& add_logged_message) +{ + if (_header_complete && _storage_config == StorageConfig::Header) { + return; + } + if (_subscriptions.find(add_logged_message.msgId()) != _subscriptions.end()) { + throw ParsingException("Duplicate AddLoggedMessage message ID"); + } + _subscriptions.insert({add_logged_message.msgId(), {add_logged_message, {}}}); +} +void DataContainer::logging(const Logging& logging) +{ + if (_header_complete && _storage_config == StorageConfig::Header) { + return; + } + _logging.emplace_back(std::move(logging)); +} +void DataContainer::data(const Data& data) +{ + if (_storage_config == StorageConfig::Header) { + return; + } + const auto& iter = _subscriptions.find(data.msgId()); + if (iter == _subscriptions.end()) { + throw ParsingException("Invalid subscription"); + } + iter->second.data.emplace_back(std::move(data)); +} +void DataContainer::dropout(const Dropout& dropout) +{ + if (_header_complete && _storage_config == StorageConfig::Header) { + return; + } + _dropouts.emplace_back(std::move(dropout)); +} +} // namespace ulog_cpp diff --git a/ulog_cpp/data_container.hpp b/ulog_cpp/data_container.hpp new file mode 100644 index 0000000..5655228 --- /dev/null +++ b/ulog_cpp/data_container.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +#include "data_handler_interface.hpp" + +namespace ulog_cpp { + +class DataContainer : public DataHandlerInterface { + public: + enum class StorageConfig { + Header, ///< keep header in memory + FullLog, ///< keep full log in memory + }; + + struct Subscription { + AddLoggedMessage add_logged_message; + std::vector data; + }; + + explicit DataContainer(StorageConfig storage_config); + virtual ~DataContainer() = default; + + void error(const std::string& msg, bool is_recoverable) override; + + void headerComplete() override; + + void fileHeader(const FileHeader& header) override; + void messageInfo(const MessageInfo& message_info) override; + void messageFormat(const MessageFormat& message_format) override; + void parameter(const Parameter& parameter) override; + void parameterDefault(const ParameterDefault& parameter_default) override; + void addLoggedMessage(const AddLoggedMessage& add_logged_message) override; + void logging(const Logging& logging) override; + void data(const Data& data) override; + void dropout(const Dropout& dropout) override; + + // Stored data + bool isHeaderComplete() const { return _header_complete; } + bool hadFatalError() const { return _had_fatal_error; } + const std::vector& parsingErrors() const { return _parsing_errors; } + const FileHeader& fileHeader() const { return _file_header; } + const std::map& messageInfo() const { return _message_info; } + const std::map>>& messageInfoMulti() const + { + return _message_info_multi; + } + const std::map& messageFormats() const { return _message_formats; } + const std::map& initialParameters() const { return _initial_parameters; } + const std::map& defaultParameters() const + { + return _default_parameters; + } + const std::vector& changedParameters() const { return _changed_parameters; } + const std::vector& logging() const { return _logging; } + const std::unordered_map& subscriptions() const { return _subscriptions; } + const std::vector& dropouts() const { return _dropouts; } + + private: + const StorageConfig _storage_config; + + bool _header_complete{false}; + bool _had_fatal_error{false}; + std::vector _parsing_errors; + + FileHeader _file_header; + std::map _message_info; + std::map>> _message_info_multi; + std::map _message_formats; + std::map _initial_parameters; + std::map _default_parameters; + std::vector _changed_parameters; + std::unordered_map _subscriptions; + std::vector _logging; + std::vector _dropouts; +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/data_handler_interface.hpp b/ulog_cpp/data_handler_interface.hpp new file mode 100644 index 0000000..f503062 --- /dev/null +++ b/ulog_cpp/data_handler_interface.hpp @@ -0,0 +1,34 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include + +#include "messages.hpp" + +namespace ulog_cpp { + +class DataHandlerInterface { + public: + virtual void headerComplete() {} + + virtual void error(const std::string& msg, bool is_recoverable) {} + + // Data methods + virtual void fileHeader(const FileHeader& header) {} + virtual void messageInfo(const MessageInfo& message_info) {} + virtual void messageFormat(const MessageFormat& message_format) {} + virtual void parameter(const Parameter& parameter) {} + virtual void parameterDefault(const ParameterDefault& parameter_default) {} + virtual void addLoggedMessage(const AddLoggedMessage& add_logged_message) {} + virtual void logging(const Logging& logging) {} + virtual void data(const Data& data) {} + virtual void dropout(const Dropout& dropout) {} + virtual void sync(const Sync& sync) {} + + private: +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/exception.hpp b/ulog_cpp/exception.hpp new file mode 100644 index 0000000..f75cf6d --- /dev/null +++ b/ulog_cpp/exception.hpp @@ -0,0 +1,37 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +namespace ulog_cpp { + +class ExceptionBase : public std::exception { + public: + explicit ExceptionBase(std::string reason) : _reason(std::move(reason)) {} + const char* what() const noexcept override { return _reason.c_str(); } + + protected: + const std::string _reason; +}; + +/** + * Data stream exception, either during serialization (writing) or deserialization (reading) + */ +class ParsingException : public ExceptionBase { + public: + explicit ParsingException(std::string reason) : ExceptionBase(std::move(reason)) {} +}; + +/** + * API is used in the wrong way, or some arguments do not satisfy some requirements + */ +class UsageException : public ExceptionBase { + public: + explicit UsageException(std::string reason) : ExceptionBase(std::move(reason)) {} +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/messages.cpp b/ulog_cpp/messages.cpp new file mode 100644 index 0000000..68796de --- /dev/null +++ b/ulog_cpp/messages.cpp @@ -0,0 +1,465 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "messages.hpp" + +#include +#include +#include + +#define CHECK_MSG_SIZE(size, min_required) \ + if ((size) < (min_required)) throw ParsingException("message too short") + +namespace ulog_cpp { + +FileHeader::FileHeader(const ulog_file_header_s& header, const ulog_message_flag_bits_s& flag_bits) + : _header(header), _flag_bits(flag_bits) +{ +} +FileHeader::FileHeader(const ulog_file_header_s& header) : _header(header), _has_flag_bits(false) +{ +} + +FileHeader::FileHeader(uint64_t timestamp, bool has_default_parameters) +{ + // Set flags + if (has_default_parameters) { + _flag_bits.compat_flags[0] |= ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK; + } + _flag_bits.msg_size = sizeof(_flag_bits) - ULOG_MSG_HEADER_LEN; + + memcpy(_header.magic, ulog_file_magic_bytes, sizeof(ulog_file_magic_bytes)); + _header.magic[7] = 1; // file version 1 + _header.timestamp = timestamp; +} + +void FileHeader::serialize(const DataWriteCB& writer) const +{ + writer(reinterpret_cast(&_header), sizeof(_header)); + if (_has_flag_bits) { + ulog_message_flag_bits_s flag_bits = _flag_bits; + flag_bits.msg_size = sizeof(flag_bits) - ULOG_MSG_HEADER_LEN; + writer(reinterpret_cast(&flag_bits), sizeof(flag_bits)); + } +} + +MessageInfo::MessageInfo(const uint8_t* msg, bool is_multi) : _is_multi(is_multi) +{ + if (is_multi) { + const ulog_message_info_multiple_s* info_multi = + reinterpret_cast(msg); + CHECK_MSG_SIZE(info_multi->msg_size, 3); + _continued = info_multi->is_continued; + if (info_multi->key_len > info_multi->msg_size - 2) { + throw ParsingException("Key too long"); // invalid + } + _field = Field(info_multi->key_value_str, info_multi->key_len); + initValues(info_multi->key_value_str + info_multi->key_len, + info_multi->msg_size - info_multi->key_len - 2); + } else { + const ulog_message_info_s* info = reinterpret_cast(msg); + CHECK_MSG_SIZE(info->msg_size, 2); + if (info->key_len > info->msg_size - 1) { + throw ParsingException("Key too long"); // invalid + } + _field = Field(info->key_value_str, info->key_len); + initValues(info->key_value_str + info->key_len, info->msg_size - info->key_len - 1); + } +} +void MessageInfo::initValues(const char* values, int len) +{ + _value.resize(len); + memcpy(_value.data(), values, len); +} +Field::Field(const char* str, int len) +{ + // Format: '[len] ' or ' ' + // Find first space + const std::string_view key_value{str, static_cast(len)}; + const std::string::size_type first_space = key_value.find(' '); + if (first_space == std::string::npos) { + throw ParsingException("Invalid key format"); + } + const std::string_view key_array = key_value.substr(0, first_space); + name = key_value.substr(first_space + 1); + // Check for arrays + const std::string::size_type bracket = key_array.find('['); + if (bracket == std::string::npos) { + type = key_array; + } else { + type = key_array.substr(0, bracket); + if (key_array[key_array.length() - 1] != ']') { + throw ParsingException("Invalid key format (missing ])"); + } + array_length = std::stoi(std::string(key_array.substr(bracket + 1))); + } +} + +const std::map Field::kBasicTypes{ + {"int8_t", 1}, {"uint8_t", 1}, {"int16_t", 2}, {"uint16_t", 2}, + {"int32_t", 4}, {"uint32_t", 4}, {"int64_t", 8}, {"uint64_t", 8}, + {"float", 4}, {"double", 8}, {"bool", 1}, {"char", 1}}; + +std::string Field::encode() const +{ + if (array_length >= 0) { + return type + '[' + std::to_string(array_length) + ']' + ' ' + name; + } + return type + ' ' + name; +} +Value::Value(const Field& field, const std::vector& value) +{ + if (field.array_length == -1 && field.type == "int8_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "uint8_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "int16_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "uint16_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "int32_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "uint32_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "int64_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "uint64_t") { + assign(value); + } else if (field.array_length == -1 && field.type == "float") { + assign(value); + } else if (field.array_length == -1 && field.type == "double") { + assign(value); + } else if (field.array_length == -1 && field.type == "bool") { + assign(value); + } else if (field.array_length == -1 && field.type == "char") { + assign(value); + } else if (field.array_length > 0 && field.type == "char") { + _value = std::string(reinterpret_cast(value.data()), value.size()); + } else { + _value = value; + } +} +template +void Value::assign(const std::vector& value) +{ + T v; + if (value.size() != sizeof(v)) throw ParsingException("Unexpected data type size"); + memcpy(&v, value.data(), sizeof(v)); + _value = v; +} +MessageInfo::MessageInfo(Field field, std::vector value, bool is_multi, bool continued) + : _field(std::move(field)), _value(std::move(value)), _continued(continued), _is_multi(is_multi) +{ +} +MessageInfo::MessageInfo(const std::string& key, int32_t value) +{ + _field.name = key; + _field.type = "int32_t"; + _value.resize(sizeof(value)); + memcpy(_value.data(), &value, sizeof(value)); +} +MessageInfo::MessageInfo(const std::string& key, float value) +{ + _field.name = key; + _field.type = "float"; + _value.resize(sizeof(value)); + memcpy(_value.data(), &value, sizeof(value)); +} +MessageInfo::MessageInfo(const std::string& key, const std::string& value) +{ + _field.name = key; + _field.type = "char"; + _field.array_length = value.length(); + _value.resize(value.length()); + memcpy(_value.data(), value.data(), value.length()); +} +void MessageInfo::serialize(const DataWriteCB& writer, ULogMessageType type) const +{ + const std::string field_encoded = _field.encode(); + if (_is_multi) { + ulog_message_info_multiple_s info_multi{}; + info_multi.is_continued = _continued; + const int msg_size = field_encoded.length() + _value.size() + 2; + if (msg_size > std::numeric_limits::max() || + field_encoded.length() > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + info_multi.key_len = field_encoded.length(); + info_multi.msg_size = msg_size; + + writer(reinterpret_cast(&info_multi), ULOG_MSG_HEADER_LEN + 2); + writer(reinterpret_cast(field_encoded.data()), field_encoded.size()); + writer(reinterpret_cast(_value.data()), _value.size()); + } else { + ulog_message_info_s info{}; + const int msg_size = field_encoded.length() + _value.size() + 1; + if (msg_size > std::numeric_limits::max() || + field_encoded.length() > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + info.key_len = field_encoded.length(); + info.msg_size = msg_size; + info.msg_type = static_cast(type); + + writer(reinterpret_cast(&info), ULOG_MSG_HEADER_LEN + 1); + writer(reinterpret_cast(field_encoded.data()), field_encoded.size()); + writer(reinterpret_cast(_value.data()), _value.size()); + } +} +MessageFormat::MessageFormat(const uint8_t* msg) +{ + const ulog_message_format_s* format = reinterpret_cast(msg); + // Format: :;; ... + auto format_str = std::string_view(format->format, format->msg_size); + const std::string::size_type colon = format_str.find(':'); + if (colon == std::string::npos) { + throw ParsingException("Invalid message format (no :)"); // invalid + } + _name = format_str.substr(0, colon); + format_str = format_str.substr(colon + 1); + while (!format_str.empty()) { + const std::string::size_type semicolon = format_str.find(';'); + if (semicolon == std::string::npos) { + throw ParsingException("Invalid message format (no ;)"); // invalid + } + _fields.emplace_back(format_str.data(), semicolon); + format_str = format_str.substr(semicolon + 1); + } +} +MessageFormat::MessageFormat(std::string name, std::vector fields) + : _name(std::move(name)), _fields(std::move(fields)) +{ +} +void MessageFormat::serialize(const DataWriteCB& writer) const +{ + std::string format_str = _name + ':'; + + for (const auto& field : _fields) { + format_str += field.encode() + ';'; + } + + ulog_message_format_s format; + const int msg_size = format_str.length(); + if (msg_size > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + format.msg_size = msg_size; + writer(reinterpret_cast(&format), ULOG_MSG_HEADER_LEN); + writer(reinterpret_cast(format_str.data()), format_str.size()); +} + +ParameterDefault::ParameterDefault(const uint8_t* msg) +{ + const ulog_message_parameter_default_s* param_default = + reinterpret_cast(msg); + CHECK_MSG_SIZE(param_default->msg_size, 3); + _default_types = param_default->default_types; + if (param_default->key_len > param_default->msg_size - 2) { + throw ParsingException("Key too long"); // invalid + } + _field = Field(param_default->key_value_str, param_default->key_len); + initValues(param_default->key_value_str + param_default->key_len, + param_default->msg_size - param_default->key_len - 2); +} +void ParameterDefault::initValues(const char* values, int len) +{ + _value.resize(len); + memcpy(_value.data(), values, len); +} +ParameterDefault::ParameterDefault(Field field, std::vector value, + ulog_parameter_default_type_t default_types) + : _field(std::move(field)), _value(std::move(value)), _default_types(default_types) +{ +} +void ParameterDefault::serialize(const DataWriteCB& writer) const +{ + const std::string field_encoded = _field.encode(); + ulog_message_parameter_default_s parameter_default{}; + const int msg_size = field_encoded.length() + _value.size() + 2; + if (msg_size > std::numeric_limits::max() || + field_encoded.length() > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + parameter_default.key_len = field_encoded.length(); + parameter_default.msg_size = msg_size; + parameter_default.default_types = _default_types; + + writer(reinterpret_cast(¶meter_default), ULOG_MSG_HEADER_LEN + 2); + writer(reinterpret_cast(field_encoded.data()), field_encoded.size()); + writer(reinterpret_cast(_value.data()), _value.size()); +} + +AddLoggedMessage::AddLoggedMessage(const uint8_t* msg) +{ + const ulog_message_add_logged_s* add_logged = + reinterpret_cast(msg); + CHECK_MSG_SIZE(add_logged->msg_size, 4); + _multi_id = add_logged->multi_id; + _msg_id = add_logged->msg_id; + _message_name = std::string(add_logged->message_name, add_logged->msg_size - 3); +} +AddLoggedMessage::AddLoggedMessage(uint8_t multi_id, uint16_t msg_id, std::string message_name) + : _multi_id(multi_id), _msg_id(msg_id), _message_name(std::move(message_name)) +{ +} +void AddLoggedMessage::serialize(const DataWriteCB& writer) const +{ + ulog_message_add_logged_s add_logged; + const int msg_size = _message_name.size() + 3; + if (msg_size > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + add_logged.multi_id = _multi_id; + add_logged.msg_id = _msg_id; + add_logged.msg_size = msg_size; + + writer(reinterpret_cast(&add_logged), ULOG_MSG_HEADER_LEN + 3); + writer(reinterpret_cast(_message_name.data()), _message_name.size()); +} + +Logging::Logging(const uint8_t* msg, bool is_tagged) : _has_tag(is_tagged) +{ + uint8_t log_level{}; + if (is_tagged) { + const ulog_message_logging_tagged_s* logging = + reinterpret_cast(msg); + CHECK_MSG_SIZE(logging->msg_size, 12); + log_level = logging->log_level; + _tag = logging->tag; + _timestamp = logging->timestamp; + _message = std::string(logging->message, logging->msg_size - 11); + } else { + const ulog_message_logging_s* logging = reinterpret_cast(msg); + CHECK_MSG_SIZE(logging->msg_size, 10); + log_level = logging->log_level; + _timestamp = logging->timestamp; + _message = std::string(logging->message, logging->msg_size - 9); + } + if (log_level < static_cast(Level::Emergency) || + log_level > static_cast(Level::Debug)) { + _log_level = Level::Debug; + } else { + _log_level = static_cast(log_level); + } +} +Logging::Logging(Level level, std::string message, uint64_t timestamp) + : _log_level(level), _timestamp(timestamp), _message(std::move(message)) +{ +} +std::string Logging::logLevelStr() const +{ + switch (_log_level) { + case Level::Emergency: + return "Emergency"; + case Level::Alert: + return "Alert"; + case Level::Critical: + return "Critical"; + case Level::Error: + return "Error"; + case Level::Warning: + return "Warning"; + case Level::Notice: + return "Notice"; + case Level::Info: + return "Info"; + case Level::Debug: + return "Debug"; + } + return "unknown"; +} +void Logging::serialize(const DataWriteCB& writer) const +{ + if (_has_tag) { + ulog_message_logging_tagged_s logging; + const int msg_size = _message.size() + 11; + if (msg_size > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + logging.log_level = static_cast(_log_level); + logging.tag = _tag; + logging.timestamp = _timestamp; + logging.msg_size = msg_size; + + writer(reinterpret_cast(&logging), ULOG_MSG_HEADER_LEN + 11); + writer(reinterpret_cast(_message.data()), _message.size()); + } else { + ulog_message_logging_s logging; + const int msg_size = _message.size() + 9; + if (msg_size > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + logging.log_level = static_cast(_log_level); + logging.timestamp = _timestamp; + logging.msg_size = msg_size; + + writer(reinterpret_cast(&logging), ULOG_MSG_HEADER_LEN + 9); + writer(reinterpret_cast(_message.data()), _message.size()); + } +} + +Data::Data(const uint8_t* msg) +{ + const ulog_message_data_s* msg_data = reinterpret_cast(msg); + CHECK_MSG_SIZE(msg_data->msg_size, 3); + _msg_id = msg_data->msg_id; + const int data_len = msg_data->msg_size - 2; + _data.resize(data_len); + memcpy(_data.data(), &msg_data->msg_id + 1, data_len); +} +Data::Data(uint16_t msg_id, std::vector data) : _msg_id(msg_id), _data(std::move(data)) +{ +} +void Data::serialize(const DataWriteCB& writer) const +{ + ulog_message_data_s data_msg; + const int msg_size = _data.size() + 2; + if (msg_size > std::numeric_limits::max()) { + throw ParsingException("message too long"); + } + data_msg.msg_id = _msg_id; + data_msg.msg_size = msg_size; + + writer(reinterpret_cast(&data_msg), ULOG_MSG_HEADER_LEN + 2); + writer(reinterpret_cast(_data.data()), _data.size()); +} + +Dropout::Dropout(const uint8_t* msg) +{ + const ulog_message_dropout_s* dropout = reinterpret_cast(msg); + CHECK_MSG_SIZE(dropout->msg_size, 2); + _duration_ms = dropout->duration; +} +Dropout::Dropout(uint16_t duration_ms) : _duration_ms(duration_ms) +{ +} +void Dropout::serialize(const DataWriteCB& writer) const +{ + ulog_message_dropout_s dropout; + const int msg_size = sizeof(dropout) - ULOG_MSG_HEADER_LEN; + dropout.duration = _duration_ms; + dropout.msg_size = msg_size; + + writer(reinterpret_cast(&dropout), sizeof(dropout)); +} +Sync::Sync(const uint8_t* msg) +{ + const ulog_message_sync_s* sync = reinterpret_cast(msg); + CHECK_MSG_SIZE(sync->msg_size, sizeof(kSyncMagicBytes)); + if (memcmp(sync->sync_magic, kSyncMagicBytes, sizeof(kSyncMagicBytes)) != 0) { + throw ParsingException("Invalid sync magic bytes"); + } +} +void Sync::serialize(const DataWriteCB& writer) const +{ + ulog_message_sync_s sync; + const int msg_size = sizeof(sync) - ULOG_MSG_HEADER_LEN; + static_assert(sizeof(sync.sync_magic) == sizeof(kSyncMagicBytes)); + memcpy(sync.sync_magic, kSyncMagicBytes, sizeof(kSyncMagicBytes)); + sync.msg_size = msg_size; + + writer(reinterpret_cast(&sync), sizeof(sync)); +} +} // namespace ulog_cpp diff --git a/ulog_cpp/messages.hpp b/ulog_cpp/messages.hpp new file mode 100644 index 0000000..c5bce09 --- /dev/null +++ b/ulog_cpp/messages.hpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "exception.hpp" +#include "raw_messages.hpp" + +namespace ulog_cpp { + +// ULog format: https://docs.px4.io/main/en/dev_log/ulog_file_format.html + +using DataWriteCB = std::function; + +class FileHeader { + public: + FileHeader(const ulog_file_header_s& header, const ulog_message_flag_bits_s& flag_bits); + explicit FileHeader(const ulog_file_header_s& header); + + explicit FileHeader(uint64_t timestamp = 0, bool has_default_parameters = false); + + const ulog_file_header_s& header() const { return _header; } + const ulog_message_flag_bits_s& flagBits() const { return _flag_bits; } + + void serialize(const DataWriteCB& writer) const; + + bool operator==(const FileHeader& h) const + { + return memcmp(&_header, &h._header, sizeof(_header)) == 0 && + (!_has_flag_bits || memcmp(&_flag_bits, &h._flag_bits, sizeof(_flag_bits)) == 0); + } + + private: + ulog_file_header_s _header{}; + ulog_message_flag_bits_s _flag_bits{}; + bool _has_flag_bits{true}; +}; + +struct Field { + Field() = default; + Field(const char* str, int len); + + static const std::map kBasicTypes; + + Field(std::string type_str, std::string name_str, int array_length_int = -1) + : type(std::move(type_str)), array_length(array_length_int), name(std::move(name_str)) + { + } + + std::string encode() const; + + bool operator==(const Field& field) const + { + return type == field.type && array_length == field.array_length && name == field.name; + } + + std::string type; + int array_length{-1}; ///< -1 means not-an-array + std::string name; +}; + +class Value { + public: + using ValueType = + std::variant>; + Value(const Field& field, const std::vector& value); + + const ValueType& data() const { return _value; } + + private: + template + void assign(const std::vector& value); + ValueType _value; +}; + +class MessageInfo { + public: + explicit MessageInfo(const uint8_t* msg, bool is_multi = false); + + MessageInfo(Field field, std::vector value, bool is_multi = false, + bool continued = false); + + MessageInfo(const std::string& key, const std::string& value); + MessageInfo(const std::string& key, int32_t value); + MessageInfo(const std::string& key, float value); + + const Field& field() const { return _field; } + std::vector& valueRaw() { return _value; } + const std::vector& valueRaw() const { return _value; } + Value value() const { return Value(_field, _value); } + bool isContinued() const { return _continued; } + bool isMulti() const { return _is_multi; } + + void serialize(const DataWriteCB& writer, ULogMessageType type = ULogMessageType::INFO) const; + + bool operator==(const MessageInfo& info) const + { + return _field == info._field && _value == info._value && _continued == info._continued && + _is_multi == info._is_multi; + } + + private: + void initValues(const char* values, int len); + Field _field{}; + std::vector _value; + bool _continued{false}; + bool _is_multi{false}; +}; + +class MessageFormat { + public: + explicit MessageFormat(const uint8_t* msg); + + explicit MessageFormat(std::string name, std::vector fields); + + const std::string& name() const { return _name; } + const std::vector& fields() const { return _fields; } + + void serialize(const DataWriteCB& writer) const; + bool operator==(const MessageFormat& format) const + { + return _name == format._name && _fields == format._fields; + } + + private: + std::string _name; + std::vector _fields; +}; + +using Parameter = MessageInfo; + +class ParameterDefault { + public: + explicit ParameterDefault(const uint8_t* msg); + + ParameterDefault(Field field, std::vector value, + ulog_parameter_default_type_t default_types); + + const Field& field() const { return _field; } + const std::vector& valueRaw() const { return _value; } + Value value() const { return Value(_field, _value); } + + void serialize(const DataWriteCB& writer) const; + + ulog_parameter_default_type_t defaultType() const { return _default_types; } + + private: + void initValues(const char* values, int len); + Field _field; + std::vector _value; + ulog_parameter_default_type_t _default_types{}; +}; + +class AddLoggedMessage { + public: + explicit AddLoggedMessage(const uint8_t* msg); + + AddLoggedMessage(uint8_t multi_id, uint16_t msg_id, std::string message_name); + + const std::string& messageName() const { return _message_name; } + uint8_t multiId() const { return _multi_id; } + uint16_t msgId() const { return _msg_id; } + + void serialize(const DataWriteCB& writer) const; + + private: + uint8_t _multi_id{}; + uint16_t _msg_id{}; + std::string _message_name; +}; + +class Logging { + public: + enum class Level : uint8_t { + Emergency = '0', + Alert = '1', + Critical = '2', + Error = '3', + Warning = '4', + Notice = '5', + Info = '6', + Debug = '7' + }; + explicit Logging(const uint8_t* msg, bool is_tagged = false); + + Logging(Level level, std::string message, uint64_t timestamp); + + Level logLevel() const { return _log_level; } + std::string logLevelStr() const; + uint16_t tag() const { return _tag; } + bool hasTag() const { return _has_tag; } + uint64_t timestamp() const { return _timestamp; } + const std::string& message() const { return _message; } + + void serialize(const DataWriteCB& writer) const; + + bool operator==(const Logging& logging) const + { + return _log_level == logging._log_level && _tag == logging._tag && + _has_tag == logging._has_tag && _timestamp == logging._timestamp && + _message == logging._message; + } + + private: + Level _log_level{}; + uint16_t _tag{}; + bool _has_tag{false}; + uint64_t _timestamp{}; + std::string _message; +}; + +class Data { + public: + explicit Data(const uint8_t* msg); + + Data(uint16_t msg_id, std::vector data); + + uint16_t msgId() const { return _msg_id; } + const std::vector& data() const { return _data; } + + void serialize(const DataWriteCB& writer) const; + + bool operator==(const Data& data) const { return _msg_id == data._msg_id && _data == data._data; } + + private: + uint16_t _msg_id{}; + std::vector _data; +}; + +class Dropout { + public: + explicit Dropout(const uint8_t* msg); + + explicit Dropout(uint16_t duration_ms); + + uint16_t durationMs() const { return _duration_ms; } + + void serialize(const DataWriteCB& writer) const; + + private: + uint16_t _duration_ms{}; +}; + +class Sync { + public: + explicit Sync(const uint8_t* msg); + + explicit Sync() = default; + + void serialize(const DataWriteCB& writer) const; + + private: + static constexpr uint8_t kSyncMagicBytes[] = {0x2F, 0x73, 0x13, 0x20, 0x25, 0x0C, 0xBB, 0x12}; +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/raw_messages.hpp b/ulog_cpp/raw_messages.hpp new file mode 100644 index 0000000..be30938 --- /dev/null +++ b/ulog_cpp/raw_messages.hpp @@ -0,0 +1,286 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include + +namespace ulog_cpp { + +// NOLINTBEGIN(*) Keep the original naming + +enum class ULogMessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + INFO_MULTIPLE = 'M', + PARAMETER = 'P', + PARAMETER_DEFAULT = 'Q', + ADD_LOGGED_MSG = 'A', + REMOVE_LOGGED_MSG = 'R', + SYNC = 'S', + DROPOUT = 'O', + LOGGING = 'L', + LOGGING_TAGGED = 'C', + FLAG_BITS = 'B', +}; + +/* declare message data structs with byte alignment (no padding) */ +#pragma pack(push, 1) + +/** first bytes of the file */ +struct ulog_file_header_s { + uint8_t magic[8]; + uint64_t timestamp; +}; + +static constexpr uint8_t ulog_file_magic_bytes[] = {'U', 'L', 'o', 'g', 0x01, 0x12, 0x35}; + +/** first bytes of the crypto key file */ +struct ulog_key_header_s { + /* magic identifying the file content */ + uint8_t magic[7]; + + /* version of this header file */ + uint8_t hdr_ver; + + /* file creation timestamp */ + uint64_t timestamp; + + /* crypto algorithm used for key exchange */ + uint8_t exchange_algorithm; + + /* encryption key index used for key exchange */ + uint8_t exchange_key; + + /* size of the key */ + uint16_t key_size; + + /* size of logfile crypto algoritm initialization data, e.g. nonce */ + uint16_t initdata_size; + + /* actual data (initdata+key) */ + uint8_t data[0]; +}; + +/** + * @brief Message Header for the ULog + * + * This header components that is in the beginning of every ULog messages that gets written into + * Definitions section as well as the Data section of the ULog file. + */ +struct ulog_message_header_s { + uint16_t msg_size; ///< Size of the message excluding the header size + uint8_t + msg_type; ///< Message type, which is one of the ASCII alphabet, defined in ULogMessageType +}; + +#define ULOG_MSG_HEADER_LEN \ + 3 // Length of the header in bytes: accounts for msg_size (2 bytes) and msg_type (1 byte) + +/** + * @brief Format Message + * + * This message describes a single ULog topic's name and it's inner fields. The inner fields can + * have the type as defined in the uORB message file (e.g. msg/action_request.msg). Including other + * uORB topics, which is the nested type case. + * + * @param format Format of the uORB topic in the format: "message_name:field0;field1;" + * Example: "action_request:uint64_t timestamp;uint8_t action;uint8_t source;uint8_t mode;uint8_t[5] + * _padding0;" + */ +struct ulog_message_format_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::FORMAT); + + char format[1500]; +}; + +/** + * @brief Subscription Message + * + * This message describes which uORB topic the logger has subscribed to. + * + * Used for indicating which msg_id corresponds to which uORB topic name (message_name) + */ +struct ulog_message_add_logged_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::ADD_LOGGED_MSG); + + uint8_t multi_id; ///< Multi instance id, if the topic is one of a multi instance uORB topic + uint16_t msg_id; ///< Message ID, an internally tracked id in the logger, which matches with the + ///< msg_id in ulog_message_data_s message + char message_name[255]; ///< Name of the uORB topic +}; + +/** + * @brief Unsubscription Message + * + * This message describes which uORB topic the logger has unsubscribed from. + */ +struct ulog_message_remove_logged_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::REMOVE_LOGGED_MSG); + + uint16_t msg_id; +}; + +/** + * @brief Sync Message + */ +struct ulog_message_sync_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::SYNC); + + uint8_t sync_magic[8]; +}; + +struct ulog_message_dropout_s { + uint16_t msg_size = sizeof(uint16_t); ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DROPOUT); + + uint16_t duration; ///< in ms +}; + +/** + * @brief Logged Data Message + * + * Includes the binary data of the uORB topic with the corresponding logger's internal msg_id. + * + * The uint8_t data[] section follows after the msg_id part, but the message struct only defines up + * until the msg_id, since the data length varies per each uORB topic (the struct size of the uORB + * message). + */ +struct ulog_message_data_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DATA); + + uint16_t msg_id; +}; + +/** + * @brief Information Message + * + * Writes the dictionary type key:value relationship of any kind of information. + * + * Example: key_value_str[] = "char[5] sys_toolchain_ver9.4.0" + */ +struct ulog_message_info_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::INFO); + + uint8_t key_len; ///< Length of the 'key' + char key_value_str[255]; ///< String with the key and value information +}; + +/** + * @brief Multiple Information Message + * + * Writes the dictionary type key:value relationship of any kind of information, but + * for the ones which has a long value that can't be contained in a single Information Message. + */ +struct ulog_message_info_multiple_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::INFO_MULTIPLE); + + uint8_t is_continued; ///< Can be used for arrays: set to 1, if this message is part of the + ///< previous with the same key + uint8_t key_len; ///< Length of the 'key' + char key_value_str[1200]; ///< String with the key and value information +}; + +/** + * @brief Logged String Message + * + * Logged string, either from PX4_INFO() macro calls or MAVLink information messages, etc. + * Useful for Debugging. + */ +struct ulog_message_logging_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::LOGGING); + + uint8_t log_level; ///< same levels as in the linux kernel + uint64_t timestamp; + char message[128]; ///< defines the maximum length of a logged message string +}; + +struct ulog_message_logging_tagged_s { + uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::LOGGING_TAGGED); + + uint8_t log_level; ///< same levels as in the linux kernel + uint16_t tag; + uint64_t timestamp; + char message[128]; ///< defines the maximum length of a logged message string +}; + +/** + * @brief Parameter Message + * + * Includes a parameter value information in the format " " + */ +struct ulog_message_parameter_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER); + + uint8_t key_len; + char key_value_str[255]; ///< String with the key and value information +}; + +/** + * @brief ULog default parameter type flags + * + * These flags indicate whether the default parameter defined in the + * ulog_message_parameter_default_s message is System's default or the User setup's default value. + * As user can set a custom default parameter for each vehicle setup that overrides the system's + * default value. + */ +enum class ulog_parameter_default_type_t : uint8_t { + system = (1 << 0), ///< System wide default parameter + current_setup = (1 << 1) ///< Custom setup default parameter +}; + +inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, + ulog_parameter_default_type_t b) +{ + return static_cast(static_cast(a) | + static_cast(b)); +} + +/** + * @brief Default Parameter Message + * + * This message defines a parameter name and it's default value. Which is useful for figuring out in + * ULog analyzer which Parameters are non-default (User modified), which can give a quick insight + * into how the vehicle is configured, since the default parameter values are well known & + * understood (e.g. PID Tuning values) + */ +struct ulog_message_parameter_default_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); + + ulog_parameter_default_type_t default_types; + uint8_t key_len; + char key_value_str[255]; ///< String with the key and value information +}; + +#define ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK (1 << 0) + +#define ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK (1 << 0) + +struct ulog_message_flag_bits_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::FLAG_BITS); + + uint8_t compat_flags[8]; + uint8_t incompat_flags[8]; ///< @see ULOG_INCOMPAT_FLAG_* + uint64_t appended_offsets[3]; ///< file offset(s) for appended data if + ///< ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK is set +}; + +#pragma pack(pop) + +// NOLINTEND(*) + +} // namespace ulog_cpp diff --git a/ulog_cpp/reader.cpp b/ulog_cpp/reader.cpp new file mode 100644 index 0000000..c8ac8fd --- /dev/null +++ b/ulog_cpp/reader.cpp @@ -0,0 +1,400 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "reader.hpp" + +#include + +#include "raw_messages.hpp" + +#if 0 +#define DBG_PRINTF(...) printf(__VA_ARGS__) +#else +#define DBG_PRINTF(...) +#endif + +namespace ulog_cpp { + +const std::set Reader::kKnownMessageTypes{{ + ULogMessageType::FORMAT, + ULogMessageType::DATA, + ULogMessageType::INFO, + ULogMessageType::INFO_MULTIPLE, + ULogMessageType::PARAMETER, + ULogMessageType::PARAMETER_DEFAULT, + ULogMessageType::ADD_LOGGED_MSG, + ULogMessageType::REMOVE_LOGGED_MSG, + ULogMessageType::SYNC, + ULogMessageType::DROPOUT, + ULogMessageType::LOGGING, + ULogMessageType::LOGGING_TAGGED, + ULogMessageType::FLAG_BITS, +}}; + +// cppcheck-suppress [uninitMemberVar,unmatchedSuppression] +Reader::Reader(std::shared_ptr data_handler_interface) + : _data_handler_interface(std::move(data_handler_interface)) +{ + // Reader assumes to run on little endian + // TODO: use std::endian from C++20 + int num = 1; + // cppcheck-suppress [knownConditionTrueFalse,unmatchedSuppression] + if (*reinterpret_cast(&num) != 1) { + _data_handler_interface->error("Reader requires little endian", false); + _state = State::InvalidData; + } + _partial_message_buffer = static_cast(malloc(kBufferSizeInit)); + _partial_message_buffer_length_capacity = kBufferSizeInit; +} + +Reader::~Reader() +{ + if (_partial_message_buffer) { + free(_partial_message_buffer); + } +} + +void Reader::readChunk(const uint8_t* data, int length) +{ + if (_state == State::InvalidData) { + return; + } + + if (_state == State::ReadMagic) { + const int num_read = readMagic(data, length); + data += num_read; + length -= num_read; + _total_num_read += num_read; + } + + if (_state == State::ReadFlagBits && length > 0) { + const int num_read = readFlagBits(data, length); + data += num_read; + length -= num_read; + _total_num_read += num_read; + } + + static constexpr int kULogHeaderLength = static_cast(sizeof(ulog_message_header_s)); + + while (length > 0 && !_need_recovery) { + // Try to get a full ulog message. There's 2 options: + // - we have some partial data in the buffer. We need to append and use that buffer + // - no partial data left. Use 'data' if it contains a full message + const uint8_t* ulog_message = nullptr; + bool clear_from_partial_message_buffer = false; + if (_partial_message_buffer_length > 0) { + auto ensure_enough_data_in_partial_buffer = [&](int required_data) -> bool { + if (_partial_message_buffer_length < required_data) { + // Try to append + const int num_append = std::min(required_data - _partial_message_buffer_length, length); + if (_partial_message_buffer_length + num_append > + _partial_message_buffer_length_capacity) { + // Overflow, resize buffer + _partial_message_buffer_length_capacity = _partial_message_buffer_length + num_append; + _partial_message_buffer = static_cast( + realloc(_partial_message_buffer, _partial_message_buffer_length_capacity)); + DBG_PRINTF("%i: resized partial buffer to %i\n", _total_num_read, + _partial_message_buffer_length_capacity); + } + memcpy(_partial_message_buffer + _partial_message_buffer_length, data, num_append); + _partial_message_buffer_length += num_append; + data += num_append; + length -= num_append; + _total_num_read += num_append; + } + return _partial_message_buffer_length >= required_data; + }; + if (ensure_enough_data_in_partial_buffer(kULogHeaderLength)) { + const ulog_message_header_s* header = + reinterpret_cast(_partial_message_buffer); + if (ensure_enough_data_in_partial_buffer(header->msg_size + kULogHeaderLength)) { + ulog_message = reinterpret_cast(_partial_message_buffer); + clear_from_partial_message_buffer = true; + } else { + // Not enough data yet (length == 0) or overflow + DBG_PRINTF("%i: not enough data (length=%i)\n", _total_num_read, length); + } + } + + } else { + int full_message_length = 0; + if (length > kULogHeaderLength) { + const ulog_message_header_s* header = reinterpret_cast(data); + if (length >= header->msg_size + kULogHeaderLength) { + full_message_length = header->msg_size + kULogHeaderLength; + } + } + if (full_message_length > 0) { + ulog_message = data; + data += full_message_length; + length -= full_message_length; + _total_num_read += full_message_length; + } else { + // Not a full message in buffer -> add to partial buffer + const int num_append = appendToPartialBuffer(data, length); + data += num_append; + length -= num_append; + _total_num_read += num_append; + } + } + + if (ulog_message) { + const ulog_message_header_s* header = + reinterpret_cast(ulog_message); + + // Check for corruption + if (header->msg_size == 0 || header->msg_type == 0) { + DBG_PRINTF("%i: Invalid msg detected\n", _total_num_read); + corruptionDetected(); + // We'll exit the loop afterwards + } else { + // Parse the message + try { + if (_state == State::ReadHeader) { + readHeaderMessage(ulog_message); + } + if (_state == State::ReadData) { + readDataMessage(ulog_message); + } + } catch (const ParsingException& exception) { + DBG_PRINTF("%i: parser exception: %s\n", _total_num_read, exception.what()); + corruptionDetected(); + } + } + + if (clear_from_partial_message_buffer) { + // In most cases this will clear the whole buffer, but in case of corruptions we might have + // more data + const int num_remove = header->msg_size + kULogHeaderLength; + memmove(_partial_message_buffer, _partial_message_buffer + num_remove, + _partial_message_buffer_length - num_remove); + _partial_message_buffer_length -= num_remove; + } + } + } + + if (_need_recovery) { + tryToRecover(data, length); + } +} + +void Reader::tryToRecover(const uint8_t* data, int length) +{ + // Try to find a valid message in 'data' by moving data into the partial buffer and search for a + // message + while (length > 0) { + const int num_append = appendToPartialBuffer(data, length); + data += num_append; + length -= num_append; + _total_num_read += num_append; + + if (_partial_message_buffer_length >= static_cast(sizeof(ulog_message_header_s))) { + bool found = false; + int index = 0; + // If the partial buffer was already full, skip the first index, otherwise we risk infinite + // recursion + if (num_append == 0) { + index = 1; + } + for (; + index < _partial_message_buffer_length - static_cast(sizeof(ulog_message_header_s)); + ++index) { + const ulog_message_header_s* header = + reinterpret_cast(_partial_message_buffer + index); + // Try to use it if it looks sane (we could also check for a SYNC message) + if (header->msg_size != 0 && header->msg_type != 0 && header->msg_size < 10000 && + kKnownMessageTypes.find(static_cast(header->msg_type)) != + kKnownMessageTypes.end()) { + found = true; + break; + } + } + + // Discard unused data + if (index > 0) { + memmove(_partial_message_buffer, _partial_message_buffer + index, + _partial_message_buffer_length - index); + _partial_message_buffer_length -= index; + } + + if (found) { + DBG_PRINTF( + "%i: recovered, recursive call (index = %i, length = %i, partial buf len = %i)\n", + _total_num_read, index, length, _partial_message_buffer_length); + _need_recovery = false; + readChunk(data, length); + + return; + } + DBG_PRINTF("%i: no valid msg found (length = %i, partial buf len = %i)\n", _total_num_read, + length, _partial_message_buffer_length); + } + } +} + +void Reader::corruptionDetected() +{ + if (!_corruption_reported) { + _data_handler_interface->error("Message corruption detected", true); + _corruption_reported = true; + } + _need_recovery = true; +} + +int Reader::appendToPartialBuffer(const uint8_t* data, int length) +{ + const int num_append = + std::min(length, _partial_message_buffer_length_capacity - _partial_message_buffer_length); + memcpy(_partial_message_buffer + _partial_message_buffer_length, data, num_append); + _partial_message_buffer_length += num_append; + return num_append; +} + +int Reader::readMagic(const uint8_t* data, int length) +{ + // Assume we read the whole magic in one piece. If needed, we could handle reading it in multiple + // bits. Note that this could also happen for truncated files. + if (length < static_cast(sizeof(ulog_file_header_s))) { + _data_handler_interface->error("Not enough data to read file magic", false); + _state = State::InvalidData; + return 0; + } + + const ulog_file_header_s* header = reinterpret_cast(data); + + // Check magic bytes + if (memcmp(header->magic, ulog_file_magic_bytes, sizeof(ulog_file_magic_bytes)) != 0) { + _data_handler_interface->error("Invalid file format (incorrect header bytes)", false); + _state = State::InvalidData; + return 0; + } + + _state = State::ReadFlagBits; + _file_header = *header; + + return sizeof(ulog_file_header_s); +} + +int Reader::readFlagBits(const uint8_t* data, int length) +{ + // Assume we read the whole flags in one piece (for simplicity of the parser) + int ret = 0; + if (length < static_cast(sizeof(ulog_message_flag_bits_s))) { + _data_handler_interface->error("Not enough data to read file flags", false); + _state = State::InvalidData; + return 0; + } + // This message is optional and follows directly the file magic + const ulog_message_flag_bits_s* flag_bits = + reinterpret_cast(data); + if (static_cast(flag_bits->msg_type) == ULogMessageType::FLAG_BITS) { + // This is expected to be the first message after the file magic + if (flag_bits->appended_offsets[0] != 0) { + // TODO: handle appended data + _data_handler_interface->error("File contains appended offsets - this is not supported", + true); + } + // Check incompat flags + bool has_incompat_flags = false; + if (flag_bits->incompat_flags[0] & ~(ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK)) { + has_incompat_flags = true; + } + for (unsigned i = 1; + i < sizeof(flag_bits->incompat_flags) / sizeof(flag_bits->incompat_flags[0]); ++i) { + if (flag_bits->incompat_flags[i]) { + has_incompat_flags = true; + } + } + if (has_incompat_flags) { + _data_handler_interface->error("Unknown incompatible flag set: cannot parse the log", false); + _state = State::InvalidData; + } else { + _data_handler_interface->fileHeader({_file_header, *flag_bits}); + ret = flag_bits->msg_size + ULOG_MSG_HEADER_LEN; + _state = State::ReadHeader; + } + } else { + // Create header w/o flag bits + _data_handler_interface->fileHeader(FileHeader{_file_header}); + _state = State::ReadHeader; + } + return ret; +} + +void Reader::readHeaderMessage(const uint8_t* message) +{ + const ulog_message_header_s* header = reinterpret_cast(message); + switch (static_cast(header->msg_type)) { + case ULogMessageType::INFO: + _data_handler_interface->messageInfo(MessageInfo{message, false}); + break; + case ULogMessageType::INFO_MULTIPLE: + _data_handler_interface->messageInfo(MessageInfo{message, true}); + break; + case ULogMessageType::FORMAT: + _data_handler_interface->messageFormat(MessageFormat{message}); + break; + case ULogMessageType::PARAMETER: + _data_handler_interface->parameter(Parameter{message}); + break; + case ULogMessageType::PARAMETER_DEFAULT: + _data_handler_interface->parameterDefault(ParameterDefault{message}); + break; + case ULogMessageType::ADD_LOGGED_MSG: + case ULogMessageType::LOGGING: + case ULogMessageType::LOGGING_TAGGED: + DBG_PRINTF("%i: Header completed\n", _total_num_read); + _state = State::ReadData; + _data_handler_interface->headerComplete(); + break; + default: + DBG_PRINTF("%i: Unknown/unexpected message type in header: %i\n", _total_num_read, + header->msg_size); + break; + } +} + +void Reader::readDataMessage(const uint8_t* message) +{ + const ulog_message_header_s* header = reinterpret_cast(message); + switch (static_cast(header->msg_type)) { + case ULogMessageType::INFO: + _data_handler_interface->messageInfo(MessageInfo{message, false}); + break; + case ULogMessageType::INFO_MULTIPLE: + _data_handler_interface->messageInfo(MessageInfo{message, true}); + break; + case ULogMessageType::PARAMETER: + _data_handler_interface->parameter(Parameter{message}); + break; + case ULogMessageType::PARAMETER_DEFAULT: + _data_handler_interface->parameterDefault(ParameterDefault{message}); + break; + case ULogMessageType::ADD_LOGGED_MSG: + _data_handler_interface->addLoggedMessage(AddLoggedMessage{message}); + break; + case ULogMessageType::LOGGING: + _data_handler_interface->logging(Logging{message}); + break; + case ULogMessageType::LOGGING_TAGGED: + _data_handler_interface->logging(Logging{message, true}); + break; + case ULogMessageType::DATA: + _data_handler_interface->data(Data{message}); + break; + case ULogMessageType::DROPOUT: + _data_handler_interface->dropout(Dropout{message}); + break; + case ULogMessageType::SYNC: + _data_handler_interface->sync(Sync{message}); + break; + default: + DBG_PRINTF("%i: Unknown/unexpected message type in data: %i\n", _total_num_read, + header->msg_size); + break; + } +} + +} // namespace ulog_cpp diff --git a/ulog_cpp/reader.hpp b/ulog_cpp/reader.hpp new file mode 100644 index 0000000..d426a20 --- /dev/null +++ b/ulog_cpp/reader.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +#include "data_handler_interface.hpp" + +namespace ulog_cpp { + +/** + * Class to deserialize an ULog file. Parsed messages are passed back to a DataHandlerInterface + * class. + */ +class Reader { + public: + explicit Reader(std::shared_ptr data_handler_interface); + ~Reader(); + + /** + * Parse next chunk of serialized ULog data. Call this iteratively, e.g. over a complete file. + * data_handler_interface will be called immediately for each parsed ULog message. + */ + void readChunk(const uint8_t* data, int length); + + private: + static constexpr int kBufferSizeInit = 2048; + + static const std::set kKnownMessageTypes; + + int readMagic(const uint8_t* data, int length); + int readFlagBits(const uint8_t* data, int length); + void corruptionDetected(); + int appendToPartialBuffer(const uint8_t* data, int length); + void tryToRecover(const uint8_t* data, int length); + + void readHeaderMessage(const uint8_t* message); + void readDataMessage(const uint8_t* message); + + enum class State { + ReadMagic, + ReadFlagBits, + ReadHeader, + ReadData, + InvalidData, + }; + + State _state{State::ReadMagic}; + const std::shared_ptr _data_handler_interface; + + uint8_t* _partial_message_buffer{nullptr}; ///< contains at most one ULog message (unless + ///< _need_recovery==true) + int _partial_message_buffer_length_capacity{0}; + int _partial_message_buffer_length{0}; + + bool _need_recovery{false}; + bool _corruption_reported{false}; + + int _total_num_read{}; ///< statistics, total number of bytes read (includes current partial + ///< buffer data) + + ulog_file_header_s _file_header{}; +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/simple_writer.cpp b/ulog_cpp/simple_writer.cpp new file mode 100644 index 0000000..7fc3257 --- /dev/null +++ b/ulog_cpp/simple_writer.cpp @@ -0,0 +1,148 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "simple_writer.hpp" + +#include + +namespace ulog_cpp { + +const std::string SimpleWriter::kFormatNameRegexStr = "[a-zA-Z0-9_\\-/]+"; +const std::regex SimpleWriter::kFormatNameRegex = std::regex(std::string(kFormatNameRegexStr)); +const std::string SimpleWriter::kFieldNameRegexStr = "[a-z0-9_]+"; +const std::regex SimpleWriter::kFieldNameRegex = std::regex(std::string(kFieldNameRegexStr)); + +SimpleWriter::SimpleWriter(DataWriteCB data_write_cb, uint64_t timestamp_us) + : _writer(std::make_unique(std::move(data_write_cb))) +{ + _writer->fileHeader(FileHeader(timestamp_us)); +} + +SimpleWriter::SimpleWriter(const std::string& filename, uint64_t timestamp_us) +{ + _file = std::fopen(filename.c_str(), "wb"); + if (!_file) { + throw ParsingException("Failed to open file"); + } + + _writer = std::make_unique( + [this](const uint8_t* data, int length) { std::fwrite(data, 1, length, _file); }); + _writer->fileHeader(FileHeader(timestamp_us)); +} + +SimpleWriter::~SimpleWriter() +{ + _writer.reset(); + if (_file) { + std::fclose(_file); + } +} + +void SimpleWriter::writeMessageFormat(const std::string& name, const std::vector& fields) +{ + if (_header_complete) { + throw UsageException("Header already complete"); + } + // Ensure the first field is the 64 bit timestamp. This is a bit stricter than what ULog requires + if (fields.empty() || fields[0].name != "timestamp" || fields[0].type != "uint64_t" || + fields[0].array_length != -1) { + throw UsageException("First message field must be 'uint64_t timestamp'"); + } + if (_formats.find(name) != _formats.end()) { + throw UsageException("Duplicate format: " + name); + } + + // Validate naming pattern + if (!std::regex_match(name, kFormatNameRegex)) { + throw UsageException("Invalid name: " + name + ", valid regex: " + kFormatNameRegexStr); + } + for (const auto& field : fields) { + if (!std::regex_match(field.name, kFieldNameRegex)) { + throw UsageException("Invalid field name: " + field.name + + ", valid regex: " + kFieldNameRegexStr); + } + } + + // Check field types and verify padding + unsigned message_size = 0; + for (const auto& field : fields) { + const auto& basic_type_iter = Field::kBasicTypes.find(field.type); + if (basic_type_iter == Field::kBasicTypes.end()) { + throw UsageException("Invalid field type (nested formats are not supported): " + field.type); + } + const int array_size = field.array_length <= 0 ? 1 : field.array_length; + if (message_size % basic_type_iter->second != 0) { + throw UsageException( + "struct requires padding, reorder fields by decreasing type size. Padding before " + "field: " + + field.name); + } + message_size += array_size * basic_type_iter->second; + } + _formats[name] = Format{message_size}; + _writer->messageFormat(MessageFormat(name, fields)); +} + +void SimpleWriter::headerComplete() +{ + if (_header_complete) { + throw UsageException("Header already complete"); + } + _writer->headerComplete(); + _header_complete = true; +} + +void SimpleWriter::writeTextMessage(Logging::Level level, const std::string& message, + uint64_t timestamp) +{ + if (!_header_complete) { + throw UsageException("Header not yet complete"); + } + _writer->logging({level, message, timestamp}); +} + +void SimpleWriter::fsync() +{ + if (_file) { + fflush(_file); + ::fsync(fileno(_file)); + } +} +uint16_t SimpleWriter::writeAddLoggedMessage(const std::string& message_format_name, + uint8_t multi_id) +{ + if (!_header_complete) { + throw UsageException("Header not yet complete"); + } + const uint16_t msg_id = _subscriptions.size(); + auto format_iter = _formats.find(message_format_name); + if (format_iter == _formats.end()) { + throw UsageException("Format not found: " + message_format_name); + } + _subscriptions.push_back({format_iter->second.message_size}); + _writer->addLoggedMessage(AddLoggedMessage(multi_id, msg_id, message_format_name)); + return msg_id; +} + +void SimpleWriter::writeDataImpl(uint16_t id, const uint8_t* data, unsigned length) +{ + if (!_header_complete) { + throw UsageException("Header not yet complete"); + } + if (id >= _subscriptions.size()) { + throw UsageException("Invalid ID"); + } + const unsigned expected_size = _subscriptions[id].message_size; + // Sanity check data size. sizeof(data) can be bigger because of struct padding at the end + if (length < expected_size) { + throw UsageException("sizeof(data) is too small"); + } + std::vector data_vec; + data_vec.resize(expected_size); + memcpy(data_vec.data(), data, expected_size); + _writer->data(Data(id, std::move(data_vec))); +} + +} // namespace ulog_cpp diff --git a/ulog_cpp/simple_writer.hpp b/ulog_cpp/simple_writer.hpp new file mode 100644 index 0000000..348e85b --- /dev/null +++ b/ulog_cpp/simple_writer.hpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include + +#include "writer.hpp" + +namespace ulog_cpp { + +/** + * ULog serialization class which checks for integrity and correct calling order. + * It throws an UsageException() in case of a failed integrity check. + */ +class SimpleWriter { + public: + /** + * Constructor with a callback for writing data. + * @param data_write_cb callback for serialized ULog data + * @param timestamp_us start timestamp [us] + */ + explicit SimpleWriter(DataWriteCB data_write_cb, uint64_t timestamp_us); + /** + * Constructor to write to a file. + * @param filename ULog file to write to (will be overwritten if it exists) + * @param timestamp_us start timestamp [us] + */ + explicit SimpleWriter(const std::string& filename, uint64_t timestamp_us); + + ~SimpleWriter(); + + /** + * Write a key-value info to the header. Typically used for versioning information. + * @tparam T one of std::string, int32_t, float + * @param key (unique) name, e.g. sys_name + * @param value + */ + template + void writeInfo(const std::string& key, const T& value) + { + if (_header_complete) { + throw UsageException("Header already complete"); + } + _writer->messageInfo(ulog_cpp::MessageInfo(key, value)); + } + + /** + * Write a parameter name-value pair to the header + * @tparam T one of int32_t, float + * @param key (unique) name, e.g. PARAM_A + * @param value + */ + template + void writeParameter(const std::string& key, const T& value) + { + if (_header_complete) { + throw UsageException("Header already complete"); + } + _writer->parameter(ulog_cpp::Parameter(key, value)); + } + + /** + * Write a message format definition to the header. + * + * Supported field types: + * "int8_t", "uint8_t", "int16_t", "uint16_t", "int32_t", "uint32_t", "int64_t", "uint64_t", + * "float", "double", "bool", "char" + * + * The first field must be: {"uint64_t", "timestamp"}. + * + * Note that ULog also supports nested format definitions, which is not supported here. + * + * When aligning the fields according to a multiple of their size, there must be no padding + * between fields. The simplest way to achieve this is to order fields by decreasing size of + * their type. If incorrect, a UsageException() is thrown. + * + * @param name format name, must match the regex: "[a-zA-Z0-9_\\-/]+" + * @param fields message fields, names must match the regex: "[a-z0-9_]+" + */ + void writeMessageFormat(const std::string& name, const std::vector& fields); + + /** + * Call this to complete the header (after calling the above methods). + */ + void headerComplete(); + + /** + * Write a parameter change (@see writeParameter()) + */ + template + void writeParameterChange(const std::string& key, const T& value) + { + if (!_header_complete) { + throw UsageException("Header not yet complete"); + } + _writer->parameter(ulog_cpp::Parameter(key, value)); + } + + /** + * Create a time-series instance based on a message format definition. + * @param message_format_name Format name from writeMessageFormat() + * @param multi_id Instance id, if there's multiple + * @return message id, used for writeData() later on + */ + uint16_t writeAddLoggedMessage(const std::string& message_format_name, uint8_t multi_id = 0); + + /** + * Write a text message + */ + void writeTextMessage(Logging::Level level, const std::string& message, uint64_t timestamp); + + /** + * Write some data. The timestamp must be monotonically increasing for a given time-series (i.e. + * same id). + * @param id ID from writeAddLoggedMessage() + * @param data data according to the message format definition + */ + template + void writeData(uint16_t id, const T& data) + { + writeDataImpl(id, reinterpret_cast(&data), sizeof(data)); + } + + /** + * Flush the buffer and call fsync() on the file (only if the file-based constructor is used). + */ + void fsync(); + + private: + static const std::string kFormatNameRegexStr; + static const std::regex kFormatNameRegex; + static const std::string kFieldNameRegexStr; + static const std::regex kFieldNameRegex; + + struct Format { + unsigned message_size; + }; + struct Subscription { + unsigned message_size; + }; + + void writeDataImpl(uint16_t id, const uint8_t* data, unsigned length); + + std::unique_ptr _writer; + std::FILE* _file{nullptr}; + + bool _header_complete{false}; + std::unordered_map _formats; + std::vector _subscriptions; +}; + +} // namespace ulog_cpp diff --git a/ulog_cpp/writer.cpp b/ulog_cpp/writer.cpp new file mode 100644 index 0000000..4b026d0 --- /dev/null +++ b/ulog_cpp/writer.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "writer.hpp" + +namespace ulog_cpp { + +Writer::Writer(DataWriteCB data_write_cb) : _data_write_cb(std::move(data_write_cb)) +{ + // Writer assumes to run on little endian + // TODO: use std::endian from C++20 + int num = 1; + // cppcheck-suppress [knownConditionTrueFalse,unmatchedSuppression] + if (*reinterpret_cast(&num) != 1) { + throw UsageException("Writer requires little endian"); + } +} + +void Writer::headerComplete() +{ + _header_complete = true; +} +void Writer::fileHeader(const FileHeader& header) +{ + header.serialize(_data_write_cb); +} +void Writer::messageInfo(const MessageInfo& message_info) +{ + message_info.serialize(_data_write_cb); +} +void Writer::messageFormat(const MessageFormat& message_format) +{ + if (_header_complete) { + throw ParsingException("Header completed, cannot write formats"); + } + message_format.serialize(_data_write_cb); +} +void Writer::parameter(const Parameter& parameter) +{ + parameter.serialize(_data_write_cb, ULogMessageType::PARAMETER); +} +void Writer::parameterDefault(const ParameterDefault& parameter_default) +{ + parameter_default.serialize(_data_write_cb); +} +void Writer::addLoggedMessage(const AddLoggedMessage& add_logged_message) +{ + if (!_header_complete) { + throw ParsingException("Header not yet completed, cannot write AddLoggedMessage"); + } + add_logged_message.serialize(_data_write_cb); +} +void Writer::logging(const Logging& logging) +{ + logging.serialize(_data_write_cb); +} +void Writer::data(const Data& data) +{ + data.serialize(_data_write_cb); +} +void Writer::dropout(const Dropout& dropout) +{ + dropout.serialize(_data_write_cb); +} +void Writer::sync(const Sync& sync) +{ + sync.serialize(_data_write_cb); +} +} // namespace ulog_cpp diff --git a/ulog_cpp/writer.hpp b/ulog_cpp/writer.hpp new file mode 100644 index 0000000..1ca6656 --- /dev/null +++ b/ulog_cpp/writer.hpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +#include "data_handler_interface.hpp" + +/** + * Low-level class for serializing ULog data. This exposes the full ULog functionality, but does + * not do integrity checks. Use SimpleWriter for a simpler API with checks. + */ +namespace ulog_cpp { + +class Writer : public DataHandlerInterface { + public: + explicit Writer(DataWriteCB data_write_cb); + virtual ~Writer() = default; + + void headerComplete() override; + + void fileHeader(const FileHeader& header) override; + void messageInfo(const MessageInfo& message_info) override; + void messageFormat(const MessageFormat& message_format) override; + void parameter(const Parameter& parameter) override; + void parameterDefault(const ParameterDefault& parameter_default) override; + void addLoggedMessage(const AddLoggedMessage& add_logged_message) override; + void logging(const Logging& logging) override; + void data(const Data& data) override; + void dropout(const Dropout& dropout) override; + void sync(const Sync& sync) override; + + private: + const DataWriteCB _data_write_cb; + bool _header_complete{false}; +}; + +} // namespace ulog_cpp