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

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)

platforms/common/uORB/uORB.cpp

@@ -40,6 +40,7 @@
 
 #include "uORBManager.hpp"
 #include "uORBCommon.hpp"
+#include "uORBMessageFields.hpp"
 
 
 #include <lib/drivers/device/Device.hpp>
@@ -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;
 	}
 }

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

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 <px4_platform_common/log.h>
+
+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<uint8_t *>(&_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

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 <uORB/topics/uORBMessageFieldsGenerated.hpp>
+#include <uORB/uORB.h>
+#include <containers/Array.hpp>
+#include <cstring>
+
+#define HEATSHRINK_DYNAMIC_ALLOC 0
+#include <lib/heatshrink/heatshrink/heatshrink_decoder.h>
+
+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<orb_id_size_t, orb_compressed_max_num_orb_ids> &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<orb_id_size_t, orb_compressed_max_num_orb_id_dependencies> &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_id_size_t, orb_compressed_max_num_orb_ids> _orb_ids;
+	px4::Array<orb_id_size_t, orb_compressed_max_num_orb_id_dependencies> _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

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 <gtest/gtest.h>
+#include <containers/Bitset.hpp>
+#include <px4_platform_common/param.h>
+#include <uORB/topics/uORBTopics.hpp>
+
+#include <string>
+
+// 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<ORB_TOPICS_COUNT> formats_found;
+	px4::Bitset<ORB_TOPICS_COUNT> 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<ORB_TOPICS_COUNT> 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);
+}