mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-08 11:50:35 +08:00
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
This commit is contained in:
Beat Küng
committed by
Thomas Stastny
Thomas Stastny
parent
142e44c418
commit
1ad5a9de08
+114
-134
@@ -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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user