uorb: compress format definitions

Reduces flash usage by ~16KB.

- compress formats at build-time into a single string with all formats
- then at runtime iteratively decompress using
  https://github.com/atomicobject/heatshrink

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};

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)

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)
+

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
+

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

src/modules/logger/logger.cpp

@@ -44,12 +44,14 @@
 #include <stdlib.h>
 #include <time.h>
 
+#include <uORB/uORBMessageFields.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/uORBTopics.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/battery_status.h>
 
+#include <containers/Bitset.hpp>
 #include <drivers/drv_hrt.h>
 #include <mathlib/math/Limits.hpp>
 #include <px4_platform/cpuload.h>
@@ -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: "<chr> timestamp;<chr>[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<ORB_TOPICS_COUNT> 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();
 }

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);
 
 	/**

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<uint8_t>(ULogMessageType::FORMAT);
 
-	char format[1500];
+	char format[1600];
 };
 
 /**

src/modules/replay/Replay.cpp

@@ -47,6 +47,7 @@
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/shutdown.h>
 #include <lib/parameters/param.h>
+#include <uORB/uORBMessageFields.hpp>
 
 #include <cstring>
 #include <float.h>
@@ -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 "<chr> timestamp;<chr>[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

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;
 };