diff --git a/.gitmodules b/.gitmodules index 8b26803957..5b6e3ea2b3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -72,3 +72,6 @@ path = src/modules/zenoh/zenoh-pico url = https://github.com/px4/zenoh-pico branch = pr-zubf-werror-fix +[submodule "src/lib/heatshrink/heatshrink"] + path = src/lib/heatshrink/heatshrink + url = https://github.com/PX4/heatshrink.git diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index c9cd4fcf9f..486eac9464 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -25,6 +25,7 @@ exec find boards msg src platforms test \ -path src/lib/crypto/monocypher -prune -o \ -path src/lib/crypto/libtomcrypt -prune -o \ -path src/lib/crypto/libtommath -prune -o \ + -path src/lib/heatshrink/heatshrink -prune -o \ -path src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client -prune -o \ -path src/lib/cdrstream/cyclonedds -prune -o \ -path src/lib/cdrstream/rosidl -prune -o \ diff --git a/Tools/msg/px_generate_uorb_compressed_fields.py b/Tools/msg/px_generate_uorb_compressed_fields.py new file mode 100755 index 0000000000..bcd2287e4f --- /dev/null +++ b/Tools/msg/px_generate_uorb_compressed_fields.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python3 +############################################################################# +# +# Copyright (C) 2023 PX4 Pro Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +Generates cpp source + header files with compressed uorb topic fields from json files +""" + +import argparse +import json +import struct +from operator import itemgetter +import sys +import os + +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../src/lib/heatshrink')) +import heatshrink_encode + + +def parse_json_files(json_files: [str]) -> dict: + """Read list of json files into a dict""" + definitions = {} + for json_file in json_files: + with open(json_file, encoding='utf-8') as file_handle: + definition = json.load(file_handle) + assert definition['name'] not in definitions + definitions[definition['name']] = definition + definitions[definition['name']]['completed'] = False + + return definitions + + +def get_ordered_list_by_dependency(name: str, definitions: dict) -> [str]: + """Iterate dependency graph and create an ordered list""" + if definitions[name]['completed']: + return [] + ret = [] + # Get nested types first (DFS) + for dependency in definitions[name]['dependencies']: + ret.extend(get_ordered_list_by_dependency(dependency, definitions)) + + ret.append(name) + definitions[name]['completed'] = True + return ret + + +def get_field_definitions(names: [str], definitions: dict) -> (bytes, [str]): + """Get byte array with all definitions""" + ret = bytes() + formats_list = [] + + for name in names: + # Format as '<# orb_ids><# orb_ids dependencies' + assert len(definitions[name]['orb_ids']) < 255 + assert len(definitions[name]['dependencies']) < 255 + ret += struct.pack(' orb_id >= 0 + ret += struct.pack(' + +namespace uORB { + +static const uint8_t compressed_fields[] = { + {FIELDS} +}; + +const uint8_t* orb_compressed_message_formats() +{ + return compressed_fields; +} +unsigned orb_compressed_message_formats_size() +{ + return sizeof(compressed_fields) / sizeof(compressed_fields[0]); +} + +} // namespace uORB +'''.replace('{FIELDS}', fields_str)) + + +def c_encode(s, encoding='ascii'): + result = '' + for c in s: + if not (32 <= ord(c) < 127) or c in ('\\', '"'): + result += '\\%03o' % ord(c) + else: + result += c + return '"' + result + '"' + + +def write_fields_to_hpp_file(file_name: str, definitions: dict, window_length: int, lookahead_length: int, + format_list: [str]): + max_tokenized_field_length, max_tokenized_field_length_msg = max( + ((len(definitions[k]['fields']), k) for k in definitions), key=itemgetter(0)) + max_num_orb_ids = max(len(definitions[k]['orb_ids']) for k in definitions) + max_num_orb_id_dependencies = max(len(definitions[k]['dependencies']) for k in definitions) + + with open(file_name, 'w') as file_handle: + file_handle.write(''' +// Auto-generated from px4_generate_uorb_compressed_fields.py +#include + +namespace uORB { + + /** + * Get compressed string of all uorb message format definitions + */ +const uint8_t* orb_compressed_message_formats(); + +/** + * Get length of compressed message format definitions + */ +unsigned orb_compressed_message_formats_size(); + +static constexpr unsigned orb_tokenized_fields_max_length = {MAX_TOKENIZED_FIELD_LENGTH}; // {MAX_TOKENIZED_FIELD_LENGTH_MSG} +static constexpr unsigned orb_compressed_max_num_orb_ids = {MAX_NUM_ORB_IDS}; +static constexpr unsigned orb_compressed_max_num_orb_id_dependencies = {MAX_NUM_ORB_ID_DEPENDENCIES}; + +static constexpr unsigned orb_compressed_heatshrink_window_length = {WINDOW_LENGTH}; +static constexpr unsigned orb_compressed_heatshrink_lookahead_length = {LOOKAHEAD_LENGTH}; + +#define ORB_DECOMPRESSED_MESSAGE_FIELDS {{DECOMPRESSED_MESSAGE_FIELDS}} + +} // namespace uORB +''' + .replace('{MAX_TOKENIZED_FIELD_LENGTH}', str(max_tokenized_field_length)) + .replace('{MAX_TOKENIZED_FIELD_LENGTH_MSG}', max_tokenized_field_length_msg) + .replace('{MAX_NUM_ORB_IDS}', str(max_num_orb_ids)) + .replace('{MAX_NUM_ORB_ID_DEPENDENCIES}', str(max_num_orb_id_dependencies)) + .replace('{WINDOW_LENGTH}', str(window_length)) + .replace('{LOOKAHEAD_LENGTH}', str(lookahead_length)) + .replace('{DECOMPRESSED_MESSAGE_FIELDS}', ','.join(c_encode(x) for x in format_list)) + ) + + +def main(): + parser = argparse.ArgumentParser(description='Generate compressed uorb topic fields') + parser.add_argument('-f', dest='file', + help="json input files", + nargs="+") + parser.add_argument('--source-output-file', dest='output_cpp', + help='cpp output file to generate') + parser.add_argument('--header-output-file', dest='output_hpp', + help='hpp output file to generate') + parser.add_argument('-v', '--verbose', + action='store_true', + help="verbose output") + args = parser.parse_args() + + if args.file is not None: + definitions = parse_json_files(args.file) + + # Get array of all field definitions + names = [] + for definition in definitions: + names.extend(get_ordered_list_by_dependency(definitions[definition]['name'], definitions)) + names.reverse() # Dependent definitions must be after + assert len(names) == len(definitions) + for definition in definitions: # sanity check + assert definitions[definition]['completed'] + field_definitions, format_list = get_field_definitions(names, definitions) + + # Compress + window_size = 8 # Larger value = better compression; memory requirement (for decompression): 2 ^ window_size + lookahead = 4 + compressed_field_definitions = heatshrink_encode.encode(field_definitions, window_size, lookahead) + + if args.verbose: + print( + f'Field definitions: size: {len(field_definitions)}, reduction from compression: {len(field_definitions) - len(compressed_field_definitions)}') + + # Write cpp & hpp file + write_fields_to_cpp_file(args.output_cpp, compressed_field_definitions) + write_fields_to_hpp_file(args.output_hpp, definitions, window_size, lookahead, format_list) + + +if __name__ == "__main__": + main() diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py index fe2a0e0d33..dad0b30451 100755 --- a/Tools/msg/px_generate_uorb_topic_files.py +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -70,9 +70,8 @@ __license__ = "BSD" __email__ = "thomasgubler@gmail.com" -TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em'] -TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] -OUTPUT_FILE_EXT = ['.h', '.cpp', '.h'] +TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em', 'msg.json.em'] +TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em', None, None] INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] PACKAGE = 'px4' TOPICS_TOKEN = '# TOPICS ' @@ -105,7 +104,7 @@ def get_topics(filename): return result -def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath): +def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath, all_topics): """ Converts a single .msg file to an uorb header/source file """ @@ -155,6 +154,7 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template "msg_context": msg_context, "spec": spec, "topics": topics, + "all_topics": all_topics, } # Make sure output directory exists: @@ -162,10 +162,11 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template os.makedirs(outputdir) template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) + extension = os.path.splitext(os.path.splitext(TEMPLATE_FILE[format_idx])[0])[1] if format_idx == 2: - output_file = os.path.join(outputdir, file_base_name + OUTPUT_FILE_EXT[format_idx]) + output_file = os.path.join(outputdir, file_base_name + extension) else: - output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx]) + output_file = os.path.join(outputdir, full_type_name_snake + extension) return generate_by_template(output_file, template_file, em_globals) @@ -195,17 +196,13 @@ def generate_by_template(output_file, template_file, em_globals): return True -def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): +def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir, all_topics): # generate cpp file with topics list filenames = [] for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]: filenames.append(re.sub(r'(? @@ -72,12 +72,9 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field #include #include -@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" -@# This is used for the logger -constexpr char __orb_@(name_snake_case)_fields[] = "@( ";".join(topic_fields) );"; - @[for topic in topics]@ -ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast(ORB_ID::@topic)); +static_assert(static_cast(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch"); +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast(ORB_ID::@topic)); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/Tools/msg/templates/uorb/msg.json.em b/Tools/msg/templates/uorb/msg.json.em new file mode 100644 index 0000000000..4f6a9111ad --- /dev/null +++ b/Tools/msg/templates/uorb/msg.json.em @@ -0,0 +1,49 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) topic names +@# - all_topics (List of String) all generated topic names (sorted) +@############################################### + +@{ +import genmsg.msgs +import json + +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%name_snake_case + +sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) +struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) +topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields] + +dependencies = [] +for field in spec.parsed_fields(): + if not field.is_header: + type_name = field.type + # detect embedded types + sl_pos = type_name.find('/') + if sl_pos >= 0: # nested type + dependencies.append(field.base_type) +}@ + +{ +@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed;" + "fields": @( json.dumps(bytearray(";".join(topic_fields)+";", 'utf-8').decode('unicode_escape')) ), + "orb_ids": @( json.dumps([ all_topics.index(topic) for topic in topics]) ), + "main_orb_id": @( all_topics.index(name_snake_case) if name_snake_case in all_topics else -1 ), + "dependencies": @( json.dumps(list(set(dependencies))) ), + "name": "@( spec.full_name )" +} \ No newline at end of file diff --git a/Tools/msg/templates/uorb/uORBTopics.cpp.em b/Tools/msg/templates/uorb/uORBTopics.cpp.em index 9a087416e4..fbbee44cf0 100644 --- a/Tools/msg/templates/uorb/uORBTopics.cpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.cpp.em @@ -8,7 +8,7 @@ @# @# Context: @# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names +@# - all_topics (List) list of all topic names (sorted) @############################################### /**************************************************************************** * @@ -50,9 +50,7 @@ msg_names = list(set([mn.replace(".msg", "") for mn in msgs])) # set() filters d msg_names.sort() msgs_count = len(msg_names) -topic_names = list(set(topics)) # set() filters duplicates -topic_names.sort() -topics_count = len(topics) +topics_count = len(all_topics) }@ @[for msg_name in msg_names]@ @@ -60,8 +58,8 @@ topics_count = len(topics) @[end for] const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = { -@[for idx, topic_name in enumerate(topic_names, 1)]@ - ORB_ID(@(topic_name))@[if idx != topic_names], @[end if] +@[for idx, topic_name in enumerate(all_topics, 1)]@ + ORB_ID(@(topic_name))@[if idx != all_topics], @[end if] @[end for] }; diff --git a/Tools/msg/templates/uorb/uORBTopics.hpp.em b/Tools/msg/templates/uorb/uORBTopics.hpp.em index 425661b2fb..d0a6586d73 100644 --- a/Tools/msg/templates/uorb/uORBTopics.hpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.hpp.em @@ -7,7 +7,8 @@ @# Start of Template @# @# Context: -@# - topics (List) list of all topic names +@# - msgs (List) list of all msg files +@# - all_topics (List) list of all topic names (sorted) @############################################### /**************************************************************************** * @@ -43,9 +44,7 @@ ****************************************************************************/ @{ -topics_count = len(topics) -topic_names_all = list(set(topics)) # set() filters duplicates -topic_names_all.sort() +topics_count = len(all_topics) }@ #pragma once @@ -63,7 +62,7 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } extern const struct orb_metadata *const *orb_get_topics() __EXPORT; enum class ORB_ID : orb_id_size_t { -@[for idx, topic_name in enumerate(topic_names_all)]@ +@[for idx, topic_name in enumerate(all_topics)]@ @(topic_name) = @(idx), @[end for] INVALID diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a98c4adb61..e2cf088979 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -253,6 +253,7 @@ set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) set(uorb_headers) set(uorb_sources) set(uorb_ucdr_headers) +set(uorb_json_files) foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) @@ -264,11 +265,9 @@ foreach(msg_file ${msg_files}) list(APPEND uorb_headers ${msg_out_path}/${msg}.h) list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp) list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h) + list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json) endforeach() -# set parent scope msg_files for other modules to consume (eg topic_listener) -set(msg_files ${msg_files} PARENT_SCOPE) - # Generate uORB headers add_custom_command( OUTPUT @@ -292,6 +291,44 @@ add_custom_command( ) add_custom_target(uorb_headers DEPENDS ${uorb_headers}) +add_custom_command( + OUTPUT + ${uorb_json_files} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + --json + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${msg_source_out_path} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb + DEPENDS + ${msg_files} + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.json.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py + COMMENT "Generating uORB json files" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM +) +add_custom_target(uorb_json_files DEPENDS ${uorb_json_files}) + +set(uorb_message_fields_cpp_file ${msg_source_out_path}/uORBMessageFieldsGenerated.cpp) +set(uorb_message_fields_header_file ${msg_out_path}/uORBMessageFieldsGenerated.hpp) +add_custom_command( + OUTPUT + ${uorb_message_fields_cpp_file} + ${uorb_message_fields_header_file} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py + -f ${uorb_json_files} + --source-output-file ${uorb_message_fields_cpp_file} + --header-output-file ${uorb_message_fields_header_file} + DEPENDS + uorb_json_files + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py + COMMENT "Generating uORB compressed fields" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM +) + # Generate microcdr headers add_custom_command( OUTPUT ${uorb_ucdr_headers} @@ -334,7 +371,7 @@ add_custom_command( VERBATIM ) -add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp) +add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp ${uorb_message_fields_cpp_file}) target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 2ab49b3bd5..609d2aebe6 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -54,6 +54,8 @@ set(SRCS_COMMON uORBCommon.hpp uORBCommunicator.hpp uORBManager.hpp + uORBMessageFields.cpp + uORBMessageFields.hpp uORBUtils.cpp uORBUtils.hpp uORBDeviceMaster.hpp @@ -76,7 +78,7 @@ if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") ${SRCS_COMMON} ${SRCS_KERNEL} ) - target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm) + target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm heatshrink) target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__) # User side library in nuttx kernel/protected build @@ -102,9 +104,11 @@ else() target_link_libraries(uORB PRIVATE cdev) endif() -target_link_libraries(uORB PRIVATE uorb_msgs) +target_link_libraries(uORB PRIVATE uorb_msgs heatshrink) target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) if(PX4_TESTING) add_subdirectory(uORB_tests) endif() + +px4_add_functional_gtest(SRC uORBMessageFieldsTest.cpp LINKLIBS uORB) diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 734b8d83f1..c91ac6bcce 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -40,6 +40,7 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" +#include "uORBMessageFields.hpp" #include @@ -196,7 +197,7 @@ int orb_get_interval(int handle, unsigned *interval) const char *orb_get_c_type(unsigned char short_type) { - // this matches with the uorb o_fields generator + // this matches with the uorb type_map_short python data switch (short_type) { case 0x82: return "int8_t"; @@ -239,25 +240,29 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool const uint8_t *data_ptr = (const uint8_t *)data; int data_offset = 0; - for (int format_idx = 0; meta->o_fields[format_idx] != 0;) { - const char *end_field = strchr(meta->o_fields + format_idx, ';'); + // Find message format + char format_buffer[128]; + uORB::MessageFormatReader format_reader(format_buffer, sizeof(format_buffer)); - if (!end_field) { - PX4_ERR("Format error in %s", meta->o_fields); - return; - } + if (!format_reader.readUntilFormat(meta->o_id)) { + PX4_ERR("Failed to get uorb format"); + return; + } - const char *c_type = orb_get_c_type(meta->o_fields[format_idx]); - const int end_field_idx = end_field - meta->o_fields; + int field_length = 0; + + while (format_reader.readNextField(field_length)) { + + const char *c_type = orb_get_c_type(format_buffer[0]); int array_idx = -1; int field_name_idx = -1; - for (int field_idx = format_idx; field_idx != end_field_idx; ++field_idx) { - if (meta->o_fields[field_idx] == '[') { + for (int field_idx = 0; field_idx < field_length; ++field_idx) { + if (format_buffer[field_idx] == '[') { array_idx = field_idx + 1; - } else if (meta->o_fields[field_idx] == ' ') { + } else if (format_buffer[field_idx] == ' ') { field_name_idx = field_idx + 1; break; } @@ -266,19 +271,10 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool int array_size = 1; if (array_idx >= 0) { - array_size = strtol(meta->o_fields + array_idx, nullptr, 10); + array_size = strtol(format_buffer + array_idx, nullptr, 10); } - char field_name[80]; - size_t field_name_len = end_field_idx - field_name_idx; - - if (field_name_len >= sizeof(field_name)) { - PX4_ERR("field name too long %s (max: %u)", meta->o_fields, (unsigned)sizeof(field_name)); - return; - } - - memcpy(field_name, meta->o_fields + field_name_idx, field_name_len); - field_name[field_name_len] = '\0'; + const char *field_name = format_buffer + field_name_idx; if (c_type) { // built-in type bool dont_print = false; @@ -458,17 +454,10 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool } else { - // extract the topic name - char topic_name[80]; - const size_t topic_name_len = array_size > 1 ? array_idx - format_idx - 1 : field_name_idx - format_idx - 1; - - if (topic_name_len >= sizeof(topic_name)) { - PX4_ERR("topic name too long in %s (max: %u)", meta->o_name, (unsigned)sizeof(topic_name)); - return; - } - - memcpy(topic_name, meta->o_fields + format_idx, topic_name_len); - topic_name[topic_name_len] = '\0'; + // Get the topic name + const size_t topic_name_len = array_size > 1 ? array_idx - 1 : field_name_idx - 1; + format_buffer[topic_name_len] = '\0'; + const char *topic_name = format_buffer; // find the metadata const orb_metadata *const *topics = orb_get_topics(); @@ -499,7 +488,5 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool data_offset += found_topic->o_size; } } - - format_idx = end_field_idx + 1; } } diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 0e2abb2bcf..7abbd7a568 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -51,7 +51,6 @@ struct orb_metadata { const char *o_name; /**< unique object name */ const uint16_t o_size; /**< object size */ const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ - const char *o_fields; /**< semicolon separated list of fields (with type) */ orb_id_size_t o_id; /**< ORB_ID enum */ }; @@ -100,15 +99,13 @@ typedef const struct orb_metadata *orb_id_t; * @param _name The name of the topic. * @param _struct The structure the topic provides. * @param _size_no_padding Struct size w/o padding at the end - * @param _fields All fields in a semicolon separated list e.g: "float[3] position;bool armed" * @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum) \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum) \ const struct orb_metadata __orb_##_name = { \ #_name, \ sizeof(_struct), \ _size_no_padding, \ - _fields, \ _orb_id_enum \ }; struct hack @@ -236,7 +233,7 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; /** - * Returns the C type string from a short type in o_fields metadata, or nullptr + * Returns the C type string from a short type in message fields metadata, or nullptr * if not a short type */ const char *orb_get_c_type(unsigned char short_type); @@ -248,7 +245,6 @@ const char *orb_get_c_type(unsigned char short_type); */ void orb_print_message_internal(const struct orb_metadata *meta, const void *data, bool print_topic_name); - __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location diff --git a/platforms/common/uORB/uORBMessageFields.cpp b/platforms/common/uORB/uORBMessageFields.cpp new file mode 100644 index 0000000000..24ec09f438 --- /dev/null +++ b/platforms/common/uORB/uORBMessageFields.cpp @@ -0,0 +1,342 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include "uORBMessageFields.hpp" + +#include + +namespace uORB +{ + +MessageFormatReader::State MessageFormatReader::readMore() +{ + if (_state == State::Complete || _state == State::Failure) { + return _state; + } + + if (_buffer_length == _buffer_capacity) { + _state = State::Failure; + PX4_ERR("buffer too small"); + return _state; + } + + const uint8_t *compressed_formats = orb_compressed_message_formats(); + const unsigned compressed_formats_size = orb_compressed_message_formats_size(); + + if (_buffer_length == 0 && _compressed_formats_idx == compressed_formats_size) { + _state = State::Complete; + return _state; + } + + const unsigned max_num_iterations = 5; // Safeguard, we're not expected to do more than a few iterations + + for (unsigned iteration = 0; iteration < max_num_iterations; ++iteration) { + switch (_state) { + case State::ReadOrbIDs: { + int num_orb_ids = _buffer[0]; + const unsigned orb_ids_size = 1 + num_orb_ids * sizeof(orb_id_size_t); + + if (_buffer_length > orb_ids_size) { + int num_dependent_orb_ids = _buffer[orb_ids_size]; + const unsigned orb_ids_dependent_size = 1 + num_dependent_orb_ids * sizeof(orb_id_size_t); + + if (_buffer_length >= orb_ids_size + orb_ids_dependent_size) { + + orb_id_size_t orb_id; + _state = State::ReadingFormat; + _format_length = 0; + + _orb_ids.clear(); + + for (int i = 0; i < num_orb_ids; ++i) { + memcpy(&orb_id, &_buffer[1 + sizeof(orb_id_size_t) * i], sizeof(orb_id_size_t)); + _orb_ids.push_back(orb_id); + } + + _orb_ids_dependencies.clear(); + + for (int i = 0; i < num_dependent_orb_ids; ++i) { + memcpy(&orb_id, &_buffer[orb_ids_size + 1 + sizeof(orb_id_size_t) * i], + sizeof(orb_id_size_t)); + _orb_ids_dependencies.push_back(orb_id); + } + + memmove(_buffer, _buffer + orb_ids_size + orb_ids_dependent_size, + _buffer_length - orb_ids_size - orb_ids_dependent_size); + _buffer_length -= orb_ids_size + orb_ids_dependent_size; + + return State::ReadOrbIDs; + } + } + + if (_buffer_length == _buffer_capacity) { + _state = State::Failure; + PX4_ERR("buffer too small"); + return _state; + } + + break; + } + + case State::ReadingFormat: { + const bool got_new_data = _format_length < _buffer_length; + + for (; _format_length < _buffer_length; ++_format_length) { + if (_buffer[_format_length] == '\0') { + _state = State::FormatComplete; + return _state; + } + } + + if (got_new_data) { + return _state; + } + } + break; + + case State::FormatComplete: + if (_format_length != 0) { + PX4_ERR("Invalid API calls"); // Missing call to clearFormatFromBuffer or clearFormatAndRestoreLeftover + _state = State::Failure; + return _state; + } + + _state = State::ReadOrbIDs; + break; + + case State::Failure: + case State::Complete: + return _state; + } + + // Decompress more data + size_t count = 0; + + if (heatshrink_decoder_sink(&_hsd, &compressed_formats[_compressed_formats_idx], + compressed_formats_size - _compressed_formats_idx, &count) < 0) { + _state = State::Failure; + return _state; + } + + _compressed_formats_idx += count; + + if (_compressed_formats_idx == compressed_formats_size) { + const HSD_finish_res fres = heatshrink_decoder_finish(&_hsd); + + if (fres != HSDR_FINISH_MORE && fres != HSDR_FINISH_DONE) { + _state = State::Failure; + return _state; + } + } + + const HSD_poll_res pres = heatshrink_decoder_poll(&_hsd, reinterpret_cast(&_buffer[_buffer_length]), + _buffer_capacity - _buffer_length, &count); + _buffer_length += count; + + if (HSDR_POLL_EMPTY != pres && HSDR_POLL_MORE != pres) { + _state = State::Failure; + return _state; + } + + if (_compressed_formats_idx == compressed_formats_size) { + const HSD_finish_res fres = heatshrink_decoder_finish(&_hsd); + + if (HSDR_FINISH_DONE != fres && HSDR_FINISH_MORE != fres) { + _state = State::Failure; + return _state; + } + } + + } + + // Not expected to get here + PX4_ERR("logic error"); + _state = State::Failure; + return _state; +} + +void MessageFormatReader::clearFormatFromBuffer() +{ + if (_state == State::FormatComplete) { + ++_format_length; // Include null char + memmove(_buffer, _buffer + _format_length, _buffer_length - _format_length); + _buffer_length -= _format_length; + + } else { + // Full buffer is occupied with format + _buffer_length = 0; + } + + _format_length = 0; +} + +int MessageFormatReader::expandMessageFormat(char *format, unsigned len, unsigned buf_len) +{ + ++len; // Include null char + + int format_idx = 0; + + while (format[format_idx] != 0) { + + const char *c_type = orb_get_c_type(format[format_idx]); + + if (c_type) { + // Replace 1 char type with expanded c_type + const int c_type_len = (int)strlen(c_type); + + if (len + c_type_len - 1 > buf_len) { + return -1; + } + + memmove(format + format_idx + c_type_len, format + format_idx + 1, len - format_idx - 1); + memcpy(format + format_idx, c_type, c_type_len); + format_idx += c_type_len - 1; + len += c_type_len - 1; + } + + // Go to next field + const char *end_field = strchr(format + format_idx, ';'); + + if (!end_field) { + PX4_ERR("Format error in %s", format); + return -1; + } + + format_idx = (int)(end_field - format + 1); + } + + if (format_idx + 1 != (int)len) { + PX4_ERR("logic error"); + return -1; + } + + return format_idx; +} + +bool MessageFormatReader::readUntilFormat(orb_id_size_t orb_id) +{ + bool done = false; + bool found_format = false; + + while (!done && !found_format) { + switch (readMore()) { + case State::ReadOrbIDs: + for (const orb_id_size_t current_orb_id : orbIDs()) { + if (current_orb_id == orb_id) { + found_format = true; + } + } + + break; + + case State::ReadingFormat: + case State::FormatComplete: + clearFormatFromBuffer(); + break; + + case State::Complete: + case State::Failure: + done = true; + break; + + default: + break; + } + } + + return found_format; +} + +bool MessageFormatReader::readNextField(int &field_length) +{ + if (field_length > 0) { + // Move left-over part to beginning + ++field_length; // include null + memmove(_buffer, _buffer + field_length, _buffer_length - field_length); + _buffer_length -= field_length; + _format_length -= field_length; + } + + auto findFieldEnd = [&]() { + // Find ';' + bool found = false; + + for (field_length = 0; field_length < (int)_format_length; ++field_length) { + if (_buffer[field_length] == ';') { + _buffer[field_length] = '\0'; + found = true; + break; + } + } + + return found; + }; + + // We might still have a field in the buffer + if (findFieldEnd()) { + return true; + } + + bool done = false; + bool ret = false; + + while (!done) { + switch (readMore()) { + case State::ReadingFormat: + if (findFieldEnd()) { + ret = true; + done = true; + } + + break; + + case State::FormatComplete: { + ret = findFieldEnd(); // Expected to return true here + done = true; + break; + } + + case State::ReadOrbIDs: // Arrived at the next format -> we're done + case State::Complete: + case State::Failure: + done = true; + break; + } + } + + return ret; +} + + +} // namespace uORB diff --git a/platforms/common/uORB/uORBMessageFields.hpp b/platforms/common/uORB/uORBMessageFields.hpp new file mode 100644 index 0000000000..4891ea504b --- /dev/null +++ b/platforms/common/uORB/uORBMessageFields.hpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#define HEATSHRINK_DYNAMIC_ALLOC 0 +#include + +namespace uORB +{ + +class MessageFormatReader +{ +public: + enum class State { + ReadOrbIDs, + ReadingFormat, + FormatComplete, + Failure, + Complete + }; + + MessageFormatReader(char *buffer, unsigned buffer_capacity) + : _buffer(buffer), _buffer_capacity(buffer_capacity) + { + heatshrink_decoder_reset(&_hsd); + static_assert(orb_compressed_heatshrink_window_length == HEATSHRINK_STATIC_WINDOW_BITS, "window length mismatch"); + static_assert(orb_compressed_heatshrink_lookahead_length == HEATSHRINK_STATIC_LOOKAHEAD_BITS, + "lookahead length mismatch"); + _buffer[0] = 0; + } + + /** + * Read and decompress more data into the given buffer (from the constructor). + * Call iteratively until completed or a failure happens. + * @return current state + */ + State readMore(); + + /** + * Read until the start of a format given an ORB ID + * @return true on success + */ + bool readUntilFormat(orb_id_size_t orb_id); + + /** + * Iteratively read fields for the current format + * @param field_length [in,out] field length, set to 0 initially + * @return true while there is a field + */ + bool readNextField(int &field_length); + + /** + * Current length of the buffer + */ + uint32_t bufferLength() const { return _buffer_length; } + /** + * Clear the buffer during ReadingFormat (if it does not need to be accumulated) or FormatComplete. + * After FormatComplete either this or clearFormatAndRestoreLeftover must be called. + */ + void clearFormatFromBuffer(); + + /** + * When FormatComplete, this can be called to move the remaining part after the format to the end of the buffer, + * allowing the buffer to be modified. + * @return length of the left-over part. + */ + unsigned moveLeftoverToBufferEnd() + { + _buffer_length -= _format_length + 1; + memmove(_buffer + _buffer_capacity - _buffer_length, _buffer + _format_length + 1, _buffer_length); + return _buffer_length; + } + /** + * After calling moveLeftoverToBufferEnd(), this must be called. + */ + void clearFormatAndRestoreLeftover() + { + memmove(_buffer, _buffer + _buffer_capacity - _buffer_length, _buffer_length); + _format_length = 0; + } + + /** + * Get the (partial if ReadingFormat or complete if FormatComplete) format length in the buffer + */ + unsigned formatLength() const { return _format_length; } + + /** + * In ReadOrbIDs, ReadingFormat or FormatComplete states, this returns the orb ID's accociated with the format. + */ + const px4::Array &orbIDs() const { return _orb_ids; } + /** + * In ReadOrbIDs, ReadingFormat or FormatComplete states, this returns the dependent orb ID's accociated with the + * format (for nested format definitions). + */ + const px4::Array &orbIDsDependencies() const { return _orb_ids_dependencies; } + + /** + * Expand a tokenized format (after decompressing it) + * @param format tokenized format, expanded in-place + * @param len Length of the format, format[len] == '\0' must hold + * @param buf_len total length of format. Must be long enough for expanded format. + * @return expanded format length, or <0 on error + */ + static int expandMessageFormat(char *format, unsigned len, unsigned buf_len); + +private: + State _state{State::ReadOrbIDs}; + px4::Array _orb_ids; + px4::Array _orb_ids_dependencies; + + unsigned _compressed_formats_idx{0}; + char *_buffer{nullptr}; + const unsigned _buffer_capacity; + uint32_t _buffer_length{0}; + unsigned _format_length{0}; + + heatshrink_decoder _hsd; +}; + + +} // namespace uORB diff --git a/platforms/common/uORB/uORBMessageFieldsTest.cpp b/platforms/common/uORB/uORBMessageFieldsTest.cpp new file mode 100644 index 0000000000..03d25b9a26 --- /dev/null +++ b/platforms/common/uORB/uORBMessageFieldsTest.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "uORBMessageFields.hpp" + +#include +#include +#include +#include + +#include + +// To run: make tests TESTFILTER=uORBMessageFields + +class uORBMessageFieldsTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + } +}; + +TEST_F(uORBMessageFieldsTest, decompressed_formats_match) +{ + char buffer[1500]; + uORB::MessageFormatReader format_reader(buffer, sizeof(buffer)); + + px4::Bitset formats_found; + px4::Bitset dependencies; + + static const char *all_formats[] = ORB_DECOMPRESSED_MESSAGE_FIELDS; + int format_idx = 0; + + bool done = false; + + while (!done) { + switch (format_reader.readMore()) { + case uORB::MessageFormatReader::State::FormatComplete: { + const unsigned format_length = format_reader.formatLength(); + EXPECT_GT(format_length, 0); + + // Move the left-over (the part after the format if any) to the end of the buffer + const unsigned leftover_length = format_reader.moveLeftoverToBufferEnd(); + + for (const orb_id_size_t orb_id : format_reader.orbIDs()) { + // Ensure each orb_id is set only once + EXPECT_FALSE(formats_found[orb_id]); + formats_found.set(orb_id); + dependencies.set(orb_id, false); // Clear dependency + } + + for (const orb_id_size_t orb_id : format_reader.orbIDsDependencies()) { + dependencies.set(orb_id); + } + + // Compare format + ASSERT_LT(format_idx, sizeof(all_formats) / sizeof(all_formats[0])); + const std::string format(buffer, format_length); + const std::string format_expected(all_formats[format_idx]); + EXPECT_EQ(format, format_expected); + + const int ret = uORB::MessageFormatReader::expandMessageFormat(buffer, format_length, + sizeof(buffer) - leftover_length); + EXPECT_GE(ret, 0); + ++format_idx; + + // Move left-over back + format_reader.clearFormatAndRestoreLeftover(); + break; + } + break; + + case uORB::MessageFormatReader::State::Failure: + PX4_ERR("Failed to read formats"); + done = true; + ASSERT_FALSE(true); + break; + + case uORB::MessageFormatReader::State::Complete: + done = true; + break; + + default: + break; + } + } + + // Check that all formats are found + for (size_t i = 0; i < formats_found.size(); ++i) { + EXPECT_TRUE(formats_found[i]); + } + + // Expect dependencies to be cleared. If this is not the case, the format ordering is incorrect. + EXPECT_EQ(dependencies.count(), 0); +} + +TEST_F(uORBMessageFieldsTest, decompress_formats_iterative) +{ + char buffer[64]; + uORB::MessageFormatReader format_reader(buffer, sizeof(buffer)); + + px4::Bitset formats_found; + + static const char *all_formats[] = ORB_DECOMPRESSED_MESSAGE_FIELDS; + int format_idx = 0; + std::string current_format; + + bool done = false; + + while (!done) { + switch (format_reader.readMore()) { + case uORB::MessageFormatReader::State::ReadingFormat: + current_format += std::string(buffer, format_reader.formatLength()); + format_reader.clearFormatFromBuffer(); + break; + + case uORB::MessageFormatReader::State::FormatComplete: { + current_format += std::string(buffer, format_reader.formatLength()); + format_reader.clearFormatFromBuffer(); + + EXPECT_FALSE(current_format.empty()); + + for (const orb_id_size_t orb_id : format_reader.orbIDs()) { + // Ensure each orb_id is set only once + EXPECT_FALSE(formats_found[orb_id]); + formats_found.set(orb_id); + } + + // Compare format + ASSERT_LT(format_idx, sizeof(all_formats) / sizeof(all_formats[0])); + const std::string format_expected(all_formats[format_idx]); + EXPECT_EQ(current_format, format_expected); + + ++format_idx; + current_format.clear(); + break; + } + break; + + case uORB::MessageFormatReader::State::Failure: + PX4_ERR("Failed to read formats"); + done = true; + ASSERT_FALSE(true); + break; + + case uORB::MessageFormatReader::State::Complete: + done = true; + break; + + default: + break; + } + } + + // Check that all formats are found + for (size_t i = 0; i < formats_found.size(); ++i) { + EXPECT_TRUE(formats_found[i]); + } +} + +TEST_F(uORBMessageFieldsTest, decompress_formats_buffer_too_short) +{ + char buffer[64]; + static_assert(uORB::orb_tokenized_fields_max_length > sizeof(buffer), "Test expects smaller buffer"); + uORB::MessageFormatReader format_reader(buffer, sizeof(buffer)); + + bool done = false; + + while (!done) { + switch (format_reader.readMore()) { + case uORB::MessageFormatReader::State::Failure: + case uORB::MessageFormatReader::State::Complete: + done = true; + break; + + default: + break; + } + } + + EXPECT_EQ(format_reader.readMore(), uORB::MessageFormatReader::State::Failure); +} + +TEST_F(uORBMessageFieldsTest, decompress_specific_format) +{ + char format[512]; + char buffer[128]; + uORB::MessageFormatReader format_reader(buffer, sizeof(buffer)); + + ASSERT_TRUE(format_reader.readUntilFormat((orb_id_size_t)ORB_ID::orb_test)); + + int field_length = 0; + int format_length = 0; + + while (format_reader.readNextField(field_length)) { + format_length += snprintf(format + format_length, sizeof(buffer) - format_length - 1, "%s;", buffer); + } + + ASSERT_GT(uORB::MessageFormatReader::expandMessageFormat(format, format_length, sizeof(format)), 0); + + const std::string expected_format = "uint64_t timestamp;int32_t val;uint8_t[4] _padding0;"; + ASSERT_EQ(expected_format, format); +} diff --git a/platforms/ros2/include/uORB/uORB.h b/platforms/ros2/include/uORB/uORB.h index c329106046..2ebfeb0753 100644 --- a/platforms/ros2/include/uORB/uORB.h +++ b/platforms/ros2/include/uORB/uORB.h @@ -46,7 +46,6 @@ struct orb_metadata { const char *o_name; /**< unique object name */ const uint16_t o_size; /**< object size */ const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ - const char *o_fields; /**< semicolon separated list of fields (with type) */ uint8_t o_id; /**< ORB_ID enum */ }; diff --git a/src/include/containers/Array.hpp b/src/include/containers/Array.hpp index 2911f605f8..26372a2e0d 100644 --- a/src/include/containers/Array.hpp +++ b/src/include/containers/Array.hpp @@ -99,6 +99,12 @@ public: const T *begin() const { return &_items[0]; } const T *end() const { return &_items[_size]; } + void clear() + { + _size = 0; + _overflow = false; + } + private: T _items[N]; size_t _size{0}; diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1094ff33c1..24e680aba0 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -51,6 +51,7 @@ add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) +add_subdirectory(heatshrink EXCLUDE_FROM_ALL) add_subdirectory(hysteresis EXCLUDE_FROM_ALL) add_subdirectory(l1 EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) diff --git a/src/lib/heatshrink/CMakeLists.txt b/src/lib/heatshrink/CMakeLists.txt new file mode 100644 index 0000000000..d1ed75f061 --- /dev/null +++ b/src/lib/heatshrink/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_git_submodule(TARGET git_heatshrink PATH heatshrink) + +px4_add_library(heatshrink + heatshrink/heatshrink_decoder.c +) + +target_compile_options(heatshrink PRIVATE + ${MAX_CUSTOM_OPT_LEVEL} + -DHEATSHRINK_DYNAMIC_ALLOC=0) + diff --git a/src/lib/heatshrink/heatshrink b/src/lib/heatshrink/heatshrink new file mode 160000 index 0000000000..052e6de72f --- /dev/null +++ b/src/lib/heatshrink/heatshrink @@ -0,0 +1 @@ +Subproject commit 052e6de72f67f1777198bce98f3de62f7f3c16a0 diff --git a/src/lib/heatshrink/heatshrink_encode.py b/src/lib/heatshrink/heatshrink_encode.py new file mode 100644 index 0000000000..dc2cec8894 --- /dev/null +++ b/src/lib/heatshrink/heatshrink_encode.py @@ -0,0 +1,422 @@ +import ctypes +from enum import Enum + + +# Note: this implementation directly follows the heatshrink_encoder.c code +# (it's neither expected to be very efficient, nor is it pythonic) + +# Enum +class HSE_state(ctypes.c_int): + HSES_NOT_FULL = 0 + HSES_FILLED = 1 + HSES_SEARCH = 2 + HSES_YIELD_TAG_BIT = 3 + HSES_YIELD_LITERAL = 4 + HSES_YIELD_BR_INDEX = 5 + HSES_YIELD_BR_LENGTH = 6 + HSES_SAVE_BACKLOG = 7 + HSES_FLUSH_BITS = 8 + HSES_DONE = 9 + + +# Constants +FLAG_IS_FINISHING = 0x01 +MATCH_NOT_FOUND = 0xFFFF + +HEATSHRINK_LITERAL_MARKER = 0x01 +HEATSHRINK_BACKREF_MARKER = 0x00 + + +# Structs +class output_info(ctypes.Structure): + _fields_ = [ + ("buf", ctypes.POINTER(ctypes.c_uint8)), + ("buf_size", ctypes.c_size_t), + ("output_size", ctypes.POINTER(ctypes.c_size_t)) + ] + + +# Functions +def add_tag_bit(hse, oi, tag): + push_bits(hse, 1, tag, oi) + + +def push_bits(hse, count, bits, oi): + assert count <= 8 + current_byte = ctypes.c_uint8(hse.current_byte) + bit_index = ctypes.c_uint8(hse.bit_index) + for i in range(count - 1, -1, -1): + bit = bits & (1 << i) + if bit: + current_byte.value |= bit_index.value + bit_index.value >>= 1 + if bit_index.value == 0: + bit_index.value = 0x80 + oi.buf[oi.output_size[0]] = current_byte.value + oi.output_size[0] += 1 + current_byte.value = 0 + hse.current_byte = current_byte.value + hse.bit_index = bit_index.value + + +def push_literal_byte(hse, oi): + processed_offset = hse.match_scan_index - 1 + input_offset = get_input_offset(hse) + processed_offset + c = hse.buffer[input_offset] + push_bits(hse, 8, c, oi) + + +# Define necessary structures and enums +class heatshrink_encoder(ctypes.Structure): + _fields_ = [ + ("input_size", ctypes.c_uint16), + ("match_scan_index", ctypes.c_uint16), + ("match_length", ctypes.c_uint16), + ("match_pos", ctypes.c_uint16), + ("outgoing_bits", ctypes.c_uint16), + ("outgoing_bits_count", ctypes.c_uint8), + ("flags", ctypes.c_uint8), + ("state", ctypes.c_uint8), + ("current_byte", ctypes.c_uint8), + ("bit_index", ctypes.c_uint8), + + ("buffer", ctypes.POINTER(ctypes.c_uint8)), + ] + + def __init__(self, window_size=8, lookahead_size=4): + super().__init__() + self.window_size = window_size + self.lookahead_size = lookahead_size + self.search_index = hs_index(window_size) + self.buffer = (ctypes.c_uint8 * (2 << window_size))() + self.bit_index = 0x80 + + +class hs_index(ctypes.Structure): + _fields_ = [("index", ctypes.POINTER(ctypes.c_int16))] + + def __init__(self, window_size): + super().__init__() + self.index = (ctypes.c_int16 * (2 << window_size))() + + +class HSE_sink_res(Enum): + HSER_SINK_OK = 0 + HSER_SINK_ERROR_NULL = -1 + HSER_SINK_ERROR_MISUSE = -2 + + +class HSE_poll_res(Enum): + HSER_POLL_EMPTY = 0 + HSER_POLL_MORE = 1 + HSER_POLL_ERROR_NULL = -1 + HSER_POLL_ERROR_MISUSE = -2 + + +class HSE_finish_res(Enum): + HSER_FINISH_DONE = 0 + HSER_FINISH_MORE = 1 + HSER_FINISH_ERROR_NULL = -1 + + +def is_finishing(hse): + return hse.flags & FLAG_IS_FINISHING + + +def can_take_byte(oi): + return oi.output_size[0] < oi.buf_size + + +def get_input_buffer_size(hse): + return 1 << hse.window_size + + +def get_lookahead_size(hse): + return 1 << hse.lookahead_size + + +def get_input_offset(hse): + return get_input_buffer_size(hse) + + +def heatshrink_encoder_sink(hse, in_buf, size, input_size): + if hse is None or in_buf is None or input_size is None: + return HSE_sink_res.HSER_SINK_ERROR_NULL + + if is_finishing(hse): + return HSE_sink_res.HSER_SINK_ERROR_MISUSE + + if hse.state != HSE_state.HSES_NOT_FULL: + return HSE_sink_res.HSER_SINK_ERROR_MISUSE + + write_offset = get_input_offset(hse) + hse.input_size + ibs = get_input_buffer_size(hse) + rem = ibs - hse.input_size + cp_sz = min(rem, size) + + for i in range(cp_sz): + hse.buffer[write_offset + i] = in_buf[i] + input_size.value = cp_sz + hse.input_size += cp_sz + + if cp_sz == rem: + hse.state = HSE_state.HSES_FILLED + + return HSE_sink_res.HSER_SINK_OK + + +def do_indexing(hse): + # Build an index array I that contains flattened linked lists + # for the previous instances of every byte in the buffer. + + hsi = hse.search_index + last = [0xffff] * 256 + buf = hse.buffer + index = hsi.index + input_offset = get_input_offset(hse) + end = input_offset + hse.input_size + + for i in range(0, end): + v = buf[i] + lv = last[v] + index[i] = lv + last[v] = i + + +def heatshrink_encoder_poll(hse, out_buf, out_buf_size, output_size): + if hse is None or out_buf is None or output_size is None: + return HSE_poll_res.HSER_POLL_ERROR_NULL + + if out_buf_size == 0: + return HSE_poll_res.HSER_POLL_ERROR_MISUSE + + output_size[0] = 0 + oi = output_info() + oi.buf = out_buf + oi.buf_size = out_buf_size + oi.output_size = output_size + + while True: + in_state = hse.state + if in_state == HSE_state.HSES_NOT_FULL: + return HSE_poll_res.HSER_POLL_EMPTY + elif in_state == HSE_state.HSES_DONE: + return HSE_poll_res.HSER_POLL_EMPTY + elif in_state == HSE_state.HSES_FILLED: + do_indexing(hse) + hse.state = HSE_state.HSES_SEARCH + elif in_state == HSE_state.HSES_SEARCH: + hse.state = st_step_search(hse) + elif in_state == HSE_state.HSES_YIELD_TAG_BIT: + hse.state = st_yield_tag_bit(hse, oi) + elif in_state == HSE_state.HSES_YIELD_LITERAL: + hse.state = st_yield_literal(hse, oi) + elif in_state == HSE_state.HSES_YIELD_BR_INDEX: + hse.state = st_yield_br_index(hse, oi) + elif in_state == HSE_state.HSES_YIELD_BR_LENGTH: + hse.state = st_yield_br_length(hse, oi) + elif in_state == HSE_state.HSES_SAVE_BACKLOG: + hse.state = st_save_backlog(hse) + elif in_state == HSE_state.HSES_FLUSH_BITS: + hse.state = st_flush_bit_buffer(hse, oi) + else: + return HSE_poll_res.HSER_POLL_ERROR_MISUSE + + if hse.state == in_state: + if oi.output_size == oi.buf_size: + return HSE_poll_res.HSER_POLL_MORE + + +def heatshrink_encoder_finish(hse): + hse.flags |= FLAG_IS_FINISHING + if hse.state == HSE_state.HSES_NOT_FULL: + hse.state = HSE_state.HSES_FILLED + if hse.state == HSE_state.HSES_DONE: + return HSE_finish_res.HSER_FINISH_DONE + return HSE_finish_res.HSER_FINISH_MORE + + +def st_step_search(hse): + window_length = get_input_buffer_size(hse) + lookahead_sz = get_lookahead_size(hse) + msi = hse.match_scan_index + + fin = is_finishing(hse) + if msi > hse.input_size - (1 if fin else lookahead_sz): + return HSE_state.HSES_FLUSH_BITS if fin else HSE_state.HSES_SAVE_BACKLOG + + input_offset = get_input_offset(hse) + end = input_offset + msi + start = end - window_length + + max_possible = lookahead_sz if hse.input_size - msi >= lookahead_sz else hse.input_size - msi + + match_pos, match_length = find_longest_match(hse, start, end, max_possible) + + if match_pos == MATCH_NOT_FOUND: + hse.match_scan_index += 1 + hse.match_length = 0 + return HSE_state.HSES_YIELD_TAG_BIT + else: + hse.match_pos = match_pos + hse.match_length = match_length + return HSE_state.HSES_YIELD_TAG_BIT + + +def find_longest_match(hse, start, end, maxlen): + buf = hse.buffer + + match_maxlen = 0 + match_index = MATCH_NOT_FOUND + + needlepoint = end + + pos = hse.search_index.index[end] + buf_needlepoint_maxlen = buf[needlepoint + match_maxlen] + while pos >= start: + + if buf[pos + match_maxlen] != buf_needlepoint_maxlen: + pos = hse.search_index.index[pos] + continue + + length = 1 + for length in range(1, maxlen): + if buf[pos + length] != buf[needlepoint + length]: + break + if length > match_maxlen: + match_maxlen = length + match_index = pos + buf_needlepoint_maxlen = buf[needlepoint + match_maxlen] + if length == maxlen: + break # won't find better + + pos = hse.search_index.index[pos] + + break_even_point = 1 + hse.window_size + hse.lookahead_size + + if match_maxlen > (break_even_point // 8): + return end - match_index, match_maxlen + return MATCH_NOT_FOUND, 0 + + +def push_outgoing_bits(hse, oi): + if hse.outgoing_bits_count > 8: + count = 8 + bits = hse.outgoing_bits >> (hse.outgoing_bits_count - 8) + else: + count = hse.outgoing_bits_count + bits = hse.outgoing_bits + + if count > 0: + push_bits(hse, count, bits, oi) + hse.outgoing_bits_count -= count + return count + + +def st_yield_tag_bit(hse, oi): + if can_take_byte(oi): + if hse.match_length == 0: + add_tag_bit(hse, oi, HEATSHRINK_LITERAL_MARKER) + return HSE_state.HSES_YIELD_LITERAL + else: + add_tag_bit(hse, oi, HEATSHRINK_BACKREF_MARKER) + hse.outgoing_bits = hse.match_pos - 1 + hse.outgoing_bits_count = hse.window_size + return HSE_state.HSES_YIELD_BR_INDEX + else: + return HSE_state.HSES_YIELD_TAG_BIT + + +def st_yield_literal(hse, oi): + if can_take_byte(oi): + push_literal_byte(hse, oi) + return HSE_state.HSES_SEARCH + else: + return HSE_state.HSES_YIELD_LITERAL + + +def st_yield_br_index(hse, oi): + if can_take_byte(oi): + if push_outgoing_bits(hse, oi) > 0: + return HSE_state.HSES_YIELD_BR_INDEX + else: + hse.outgoing_bits = hse.match_length - 1 + hse.outgoing_bits_count = hse.lookahead_size + return HSE_state.HSES_YIELD_BR_LENGTH + else: + return HSE_state.HSES_YIELD_BR_INDEX + + +def st_yield_br_length(hse, oi): + if can_take_byte(oi): + if push_outgoing_bits(hse, oi) > 0: + return HSE_state.HSES_YIELD_BR_LENGTH + else: + hse.match_scan_index += hse.match_length + hse.match_length = 0 + return HSE_state.HSES_SEARCH + else: + return HSE_state.HSES_YIELD_BR_LENGTH + + +def st_save_backlog(hse): + save_backlog(hse) + return HSE_state.HSES_NOT_FULL + + +def st_flush_bit_buffer(hse, oi): + if hse.bit_index == 0x80: + return HSE_state.HSES_DONE + elif can_take_byte(oi): + oi.buf[oi.output_size[0]] = hse.current_byte + oi.output_size[0] += 1 + return HSE_state.HSES_DONE + else: + return HSE_state.HSES_FLUSH_BITS + + +def save_backlog(hse): + input_buf_sz = get_input_buffer_size(hse) + + msi = hse.match_scan_index + + rem = input_buf_sz - msi # unprocessed bytes + shift_sz = input_buf_sz + rem + + for i in range(shift_sz): + hse.buffer[i] = hse.buffer[input_buf_sz - rem + i] + + hse.match_scan_index = 0 + hse.input_size -= input_buf_sz - rem + + +def encode(data, window_size, lookahead_size): + hse = heatshrink_encoder(window_size, lookahead_size) + input_buf = (ctypes.c_uint8 * len(data))() + for i, d in enumerate(data): + input_buf[i] = d + in_size = len(input_buf) + + out_buf_size = 4 * in_size # set output buffer size a bit larger + out_buf = (ctypes.c_uint8 * out_buf_size)() + + sunk = 0 + ret = [] + while sunk < in_size: + input_size = ctypes.c_size_t(in_size) + heatshrink_encoder_sink(hse, input_buf, in_size - sunk, input_size) + input_buf = input_buf[input_size.value:] + sunk += input_size.value + if sunk == in_size: + heatshrink_encoder_finish(hse) + + poll_res = HSE_poll_res.HSER_POLL_MORE + while poll_res == HSE_poll_res.HSER_POLL_MORE: + output_size = (ctypes.c_size_t * 1)() + poll_res = heatshrink_encoder_poll(hse, out_buf, out_buf_size, output_size) + ret += list(out_buf)[0:output_size[0]] + + if sunk == in_size: + heatshrink_encoder_finish(hse) + return ret + diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt index ec07d9adae..c75622e1d7 100644 --- a/src/modules/logger/CMakeLists.txt +++ b/src/modules/logger/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE modules__logger MAIN logger PRIORITY "SCHED_PRIORITY_MAX-30" + STACK_MAIN 2500 COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} -Wno-cast-align # TODO: fix and enable diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 247c0b9e84..89b3cfe50d 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -44,12 +44,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -1659,156 +1661,134 @@ void Logger::write_console_output() } -void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats &written_formats, - ulog_message_format_s &msg, int subscription_index, int level) -{ - if (level > 3) { - // precaution: limit recursion level. If we land here it's either a bug or nested topic definitions. In the - // latter case, increase the maximum level. - PX4_ERR("max recursion level reached (%i)", level); - return; - } - - // check if we already wrote the format: either if at a previous _subscriptions index or in written_formats - for (const auto &written_format : written_formats) { - if (written_format == &meta) { - PX4_DEBUG("already added: %s", meta.o_name); - return; - } - } - - for (int i = 0; i < subscription_index; ++i) { - if (_subscriptions[i].get_topic() == &meta) { - PX4_DEBUG("already in _subscriptions: %s", meta.o_name); - return; - } - } - - PX4_DEBUG("writing format for %s", meta.o_name); - - // Write the current format (we don't need to check if we already added it to written_formats) - int format_len = snprintf(msg.format, sizeof(msg.format), "%s:", meta.o_name); - - for (int format_idx = 0; meta.o_fields[format_idx] != 0;) { - const char *end_field = strchr(meta.o_fields + format_idx, ';'); - - if (!end_field) { - PX4_ERR("Format error in %s", meta.o_fields); - return; - } - - const char *c_type = orb_get_c_type(meta.o_fields[format_idx]); - - if (c_type) { - format_len += snprintf(msg.format + format_len, sizeof(msg.format) - format_len, "%s", c_type); - ++format_idx; - } - - int len = end_field - (meta.o_fields + format_idx) + 1; - - if (len >= (int)sizeof(msg.format) - format_len) { - PX4_WARN("skip topic %s, format string is too large, max is %zu", meta.o_name, - sizeof(ulog_message_format_s::format)); - return; - } - - memcpy(msg.format + format_len, meta.o_fields + format_idx, len); - format_len += len; - format_idx += len; - } - - msg.format[format_len] = '\0'; - size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len; - msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; - - write_message(type, &msg, msg_size); - - if (level > 1 && !written_formats.push_back(&meta)) { - PX4_ERR("Array too small"); - } - - // Now go through the fields and check for nested type usages. - // o_fields looks like this for example: " timestamp;[5] array;" - const char *fmt = meta.o_fields; - - while (fmt && *fmt) { - // extract the type name - char type_name[64]; - const char *space = strchr(fmt, ' '); - - if (!space) { - PX4_ERR("invalid format %s", fmt); - break; - } - - const char *array_start = strchr(fmt, '['); // check for an array - - int type_length; - - if (array_start && array_start < space) { - type_length = array_start - fmt; - - } else { - type_length = space - fmt; - } - - if (type_length >= (int)sizeof(type_name)) { - PX4_ERR("buf len too small"); - break; - } - - memcpy(type_name, fmt, type_length); - type_name[type_length] = '\0'; - - // ignore built-in types - if (orb_get_c_type(type_name[0]) == nullptr) { - - // find orb meta for type - const orb_metadata *const *topics = orb_get_topics(); - const orb_metadata *found_topic = nullptr; - - for (size_t i = 0; i < orb_topics_count(); i++) { - if (strcmp(topics[i]->o_name, type_name) == 0) { - found_topic = topics[i]; - } - } - - if (found_topic) { - - write_format(type, *found_topic, written_formats, msg, subscription_index, level + 1); - - } else { - PX4_ERR("No definition for topic %s found", fmt); - } - } - - fmt = strchr(fmt, ';'); - - if (fmt) { ++fmt; } - } -} - void Logger::write_formats(LogType type) { _writer.lock(); - // both of these are large and thus we need to be careful in terms of stack size requirements + // This is large and thus we need to be careful in terms of stack size requirements ulog_message_format_s msg; - WrittenFormats written_formats; - // write all subscribed formats + // Write all subscribed formats int sub_count = _num_subscriptions; if (type == LogType::Mission) { sub_count = _num_mission_subs; } + // Keep a bitset of all required formats (nested definitions are added later on to the bitset) + px4::Bitset formats_to_write; + for (int i = 0; i < sub_count; ++i) { const LoggerSubscription &sub = _subscriptions[i]; - write_format(type, *sub.get_topic(), written_formats, msg, i); + + if (sub.get_topic()->o_id < formats_to_write.size()) { + formats_to_write.set(sub.get_topic()->o_id); + + } else { + PX4_ERR("logic error"); + } } - write_format(type, *_event_subscription.get_topic(), written_formats, msg, sub_count); + formats_to_write.set(_event_subscription.get_topic()->o_id); + + + static_assert(sizeof(msg.format) > uORB::orb_tokenized_fields_max_length, "uORB message definition too long"); + uORB::MessageFormatReader format_reader(msg.format, sizeof(msg.format)); + bool done = false; + + while (!done) { + switch (format_reader.readMore()) { + case uORB::MessageFormatReader::State::FormatComplete: { + unsigned format_length = format_reader.formatLength(); + // Move the left-over (the part after the format if any) to the end of the buffer + const unsigned leftover_length = format_reader.moveLeftoverToBufferEnd(); + + bool needs_expansion = true; + int last_name_length = 0; + bool format_error = false; + + for (const orb_id_size_t orb_id : format_reader.orbIDs()) { + if (orb_id >= formats_to_write.size() || !formats_to_write[orb_id]) { + continue; + } + + // Make sure to write dependencies too + for (const orb_id_size_t orb_id_dep : format_reader.orbIDsDependencies()) { + formats_to_write.set(orb_id_dep); + } + + formats_to_write.set(orb_id, false); + const orb_metadata &meta = *get_orb_meta((ORB_ID) orb_id); + + PX4_DEBUG("writing format for %s", meta.o_name); + + // Expand if needed (first time only) + if (needs_expansion) { + const int ret = uORB::MessageFormatReader::expandMessageFormat(msg.format, format_length, + sizeof(msg.format) - leftover_length); + + if (ret < 0) { + PX4_ERR("Format %s error (too long?)", meta.o_name); + format_error = true; + + } else { + format_length = ret; + } + + needs_expansion = false; + } + + // Prepend format name and ':' + const int name_length = strlen(meta.o_name) + 1; // + 1 for ':' + + if (format_length + name_length - last_name_length + 1 > sizeof(msg.format) - leftover_length) { + PX4_ERR("Format %s too long", meta.o_name); + format_error = true; + } + + if (format_error) { + break; + } + + if (last_name_length != name_length) { + memmove(msg.format + name_length, msg.format + last_name_length, + format_length + 1 - last_name_length); + msg.format[name_length - 1] = ':'; + format_length += name_length - last_name_length; + last_name_length = name_length; + } + + memcpy(msg.format, meta.o_name, name_length - 1); + + size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_length; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + write_message(type, &msg, msg_size); + } + + // Move left-over back + format_reader.clearFormatAndRestoreLeftover(); + break; + } + break; + + case uORB::MessageFormatReader::State::Failure: + PX4_ERR("Failed to read formats"); + done = true; + break; + + case uORB::MessageFormatReader::State::Complete: + done = true; + break; + + default: + break; + } + } + + if (formats_to_write.count() > 0) { + // Getting here is a bug. Maybe the ordering of nested formats is not as expected? + PX4_ERR("Not all formats written"); + } _writer.unlock(); } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 1531ac8b73..9b46801099 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -236,11 +236,6 @@ private: */ void write_header(LogType type); - /// Array to store written formats for nested definitions (only) - using WrittenFormats = Array < const orb_metadata *, 20 >; - - void write_format(LogType type, const orb_metadata &meta, WrittenFormats &written_formats, ulog_message_format_s &msg, - int subscription_index, int level = 1); void write_formats(LogType type); /** diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h index 02162d461b..d676f5222c 100644 --- a/src/modules/logger/messages.h +++ b/src/modules/logger/messages.h @@ -116,7 +116,7 @@ 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]; + char format[1600]; }; /** diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 3f05082f9a..916936114d 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -391,33 +392,30 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size) } -string Replay::parseOrbFields(const string &fields) +string Replay::getOrbFields(const orb_metadata *meta) { - string ret{}; + char format[3000]; + char buffer[512]; + uORB::MessageFormatReader format_reader(buffer, sizeof(buffer)); - // convert o_fields from " timestamp;[5] array;" to "uint64_t timestamp;int8_t[5] array;" - for (int format_idx = 0; format_idx < (int)fields.length();) { - const char *end_field = strchr(fields.c_str() + format_idx, ';'); - - if (!end_field) { - PX4_ERR("Format error in %s", fields.c_str()); - return ""; - } - - const char *c_type = orb_get_c_type(fields[format_idx]); - - if (c_type) { - string str_type = c_type; - ret += str_type; - ++format_idx; - } - - int len = end_field - (fields.c_str() + format_idx) + 1; - ret += fields.substr(format_idx, len); - format_idx += len; + if (!format_reader.readUntilFormat(meta->o_id)) { + PX4_ERR("failed to find format for topic %s", meta->o_name); + return ""; } - return ret; + int field_length = 0; + int format_length = 0; + + while (format_reader.readNextField(field_length)) { + format_length += snprintf(format + format_length, sizeof(buffer) - format_length - 1, "%s;", buffer); + } + + if (uORB::MessageFormatReader::expandMessageFormat(format, format_length, sizeof(format)) < 0) { + PX4_ERR("failed to expand message format for %s", meta->o_name); + return ""; + } + + return format; } bool @@ -455,7 +453,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) // FIXME: this should check recursively, all used nested types string file_format = _file_formats[topic_name]; - const string orb_fields = parseOrbFields(orb_meta->o_fields); + const string orb_fields = getOrbFields(orb_meta); if (file_format != orb_fields) { // check if we have a compatibility conversion available diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index ef6f0ee97b..75ee3c1095 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -297,7 +297,7 @@ private: void setUserParams(const char *filename); void readDynamicParams(const char *filename); - std::string parseOrbFields(const std::string &fields); + std::string getOrbFields(const orb_metadata *meta); static char *_replay_file; };