diff --git a/msg/templates/uorb/msg.cpp.em b/msg/templates/uorb/msg.cpp.em index f7451740f8..691b6bea67 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/msg/templates/uorb/msg.cpp.em @@ -61,7 +61,7 @@ topic_name = spec.short_name 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), field.name) for field in sorted_fields] +topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields] }@ #include diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index bc8147f0fc..d4d101f327 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -60,6 +60,23 @@ type_map = { 'char': 'char', } +type_map_short = { + # We use some range outside of alpha-numeric and special characters. + # This needs to match with orb_get_c_type() + 'int8': '\\x82', + 'int16': '\\x83', + 'int32': '\\x84', + 'int64': '\\x85', + 'uint8': '\\x86', + 'uint16': '\\x87', + 'uint32': '\\x88', + 'uint64': '\\x89', + 'float32': '\\x8a', + 'float64': '\\x8b', + 'bool': '\\x8c', + 'char': '\\x8d', +} + type_serialize_map = { 'int8': 'int8_t', 'int16': 'int16_t', @@ -204,7 +221,7 @@ def add_padding_bytes(fields, search_path): return (struct_size, num_padding_bytes) -def convert_type(spec_type): +def convert_type(spec_type, use_short_type=False): """ Convert from msg type to C type """ @@ -215,7 +232,9 @@ def convert_type(spec_type): msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) c_type = msg_type - if msg_type in type_map: + if use_short_type and msg_type in type_map_short: + c_type = type_map_short[msg_type] + elif msg_type in type_map: c_type = type_map[msg_type] if is_array: return c_type + "[" + str(array_length) + "]" diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 7e3b96904d..4f266ca489 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -172,3 +172,35 @@ int orb_get_interval(int handle, unsigned *interval) { return uORB::Manager::get_instance()->orb_get_interval(handle, interval); } + +const char *orb_get_c_type(unsigned char short_type) +{ + // this matches with the uorb o_fields generator + switch (short_type) { + case 0x82: return "int8_t"; + + case 0x83: return "int16_t"; + + case 0x84: return "int32_t"; + + case 0x85: return "int64_t"; + + case 0x86: return "uint8_t"; + + case 0x87: return "uint16_t"; + + case 0x88: return "uint32_t"; + + case 0x89: return "uint64_t"; + + case 0x8a: return "float"; + + case 0x8b: return "double"; + + case 0x8c: return "bool"; + + case 0x8d: return "char"; + } + + return nullptr; +} diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 95061c37f4..27cc9a2863 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -31,8 +31,7 @@ * ****************************************************************************/ -#ifndef _UORB_UORB_H -#define _UORB_UORB_H +#pragma once /** * @file uORB.h @@ -235,6 +234,12 @@ 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 + * if not a short type + */ +const char *orb_get_c_type(unsigned char short_type); + __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location @@ -245,4 +250,3 @@ typedef uint8_t hil_state_t; typedef uint8_t navigation_state_t; typedef uint8_t switch_pos_t; -#endif /* _UORB_UORB_H */ diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 94a9037f74..6378916c9a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -362,15 +362,6 @@ void LoggedTopics::add_mission_topic(const char *name, uint16_t interval_ms) bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance) { - size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' - - if (fields_len > sizeof(ulog_message_format_s::format)) { - PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len, - sizeof(ulog_message_format_s::format)); - - return false; - } - if (_subscriptions.count >= MAX_TOPICS_NUM) { PX4_WARN("Too many subscriptions, failed to add: %s %" PRIu8, topic->o_name, instance); return false; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 047f5f56ea..16bd6903da 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1626,7 +1626,37 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats 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:%s", meta.o_name, meta.o_fields); + 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; @@ -1637,7 +1667,7 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats } // Now go through the fields and check for nested type usages. - // o_fields looks like this for example: "uint64_t timestamp;uint8_t[5] array;" + // o_fields looks like this for example: " timestamp;[5] array;" const char *fmt = meta.o_fields; while (fmt && *fmt) { @@ -1670,20 +1700,7 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats type_name[type_length] = '\0'; // ignore built-in types - if (strcmp(type_name, "int8_t") != 0 && - strcmp(type_name, "uint8_t") != 0 && - strcmp(type_name, "int16_t") != 0 && - strcmp(type_name, "uint16_t") != 0 && - strcmp(type_name, "int16_t") != 0 && - strcmp(type_name, "uint16_t") != 0 && - strcmp(type_name, "int32_t") != 0 && - strcmp(type_name, "uint32_t") != 0 && - strcmp(type_name, "int64_t") != 0 && - strcmp(type_name, "uint64_t") != 0 && - strcmp(type_name, "float") != 0 && - strcmp(type_name, "double") != 0 && - strcmp(type_name, "bool") != 0 && - strcmp(type_name, "char") != 0) { + if (orb_get_c_type(type_name[0]) == nullptr) { // find orb meta for type const orb_metadata *const *topics = orb_get_topics(); diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 76d17430cf..800c5a4e7a 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -336,6 +336,36 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size) return true; } + +string Replay::parseOrbFields(const string &fields) +{ + string ret{}; + + // 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; + } + + return ret; +} + bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) { @@ -371,12 +401,12 @@ 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]; - std::string orb_fields(orb_meta->o_fields); + const string orb_fields = parseOrbFields(orb_meta->o_fields); if (file_format != orb_fields) { // check if we have a compatibility conversion available if (topic_name == "sensor_combined") { - if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;" + if (orb_fields == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;" "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" "uint32_t accelerometer_integral_dt" && file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;" @@ -390,9 +420,9 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) int unused; if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) && - findFieldOffset(orb_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && + findFieldOffset(orb_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) && - findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { + findFieldOffset(orb_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern, accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern); @@ -449,7 +479,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) //find the timestamp offset int field_size; - bool timestamp_found = findFieldOffset(orb_meta->o_fields, "timestamp", subscription->timestamp_offset, field_size); + bool timestamp_found = findFieldOffset(orb_fields, "timestamp", subscription->timestamp_offset, field_size); if (!timestamp_found) { delete subscription; diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 6b52c120ad..94ecc9d416 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -277,6 +277,8 @@ private: void setUserParams(const char *filename); + std::string parseOrbFields(const std::string &fields); + static char *_replay_file; };