diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template index 589d05dca4..c9bf11bb85 100644 --- a/msg/templates/uorb/msg.cpp.template +++ b/msg/templates/uorb/msg.cpp.template @@ -13,10 +13,12 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2016 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 @@ -54,7 +56,6 @@ import genmsg.msgs import gencpp uorb_struct = '%s_s'%spec.short_name -uorb_pack_func = 'pack_%s'%spec.short_name topic_name = spec.short_name type_map = { @@ -70,9 +71,6 @@ type_map = { 'float64': 'double', 'bool': 'bool', 'char': 'char', - 'fence_vertex': 'fence_vertex', - 'position_setpoint': 'position_setpoint', - 'esc_report': 'esc_report' } msgtype_size_map = { @@ -88,13 +86,8 @@ msgtype_size_map = { 'float64': 8, 'bool': 1, 'char': 1, - 'fence_vertex': 8, - 'position_setpoint': 104, - 'esc_report': 36 } -msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex'] - def convert_type(spec_type): bare_type = spec_type if '/' in spec_type: @@ -102,7 +95,9 @@ def convert_type(spec_type): bare_type = (spec_type.split('/'))[1] msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) - c_type = type_map[msg_type] + c_type = msg_type + if msg_type in type_map: + c_type = type_map[msg_type] if is_array: return c_type + "[" + str(array_length) + "]" return c_type @@ -116,9 +111,65 @@ def bare_name(msg_type): return bare.split('[')[0] def sizeof_field_type(field): - return msgtype_size_map[bare_name(field.type)] + bare_name_str = bare_name(field.type) + if bare_name_str in msgtype_size_map: + return msgtype_size_map[bare_name_str] + return 0 # this is for non-builtin types: sort them at the end + +def get_children_fields(base_type): + (package, name) = genmsg.names.package_resource_name(base_type) + tmp_msg_context = genmsg.msg_loader.MsgContext.create_default() + spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path) + sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True) + return sorted_fields + +# Add padding fields before the embedded types, at the end and calculate the +# struct size +def add_padding_bytes(fields): + struct_size = 8 # account for the timestamp + align_to = 8 # this is always 8, because of the 64bit timestamp + i = 0 + padding_idx = 0 + while i < len(fields): + field = fields[i] + if not field.is_header: + a_pos = field.type.find('[') + array_size = 1 + if field.is_array: + array_size = field.array_len + if field.is_builtin: + field.sizeof_field_type = sizeof_field_type(field) + else: + # embedded type: may need to add padding + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes != align_to: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.insert(i, padding_field) + i += 1 + children_fields = get_children_fields(field.base_type) + field.sizeof_field_type, unused = add_padding_bytes(children_fields) + struct_size += field.sizeof_field_type * array_size + i += 1 + + # add padding at the end (necessary for embedded types) + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes == align_to: + num_padding_bytes = 0 + else: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.append(padding_field) + return (struct_size, num_padding_bytes) sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) +struct_size, padding_end_size = add_padding_bytes(sorted_fields) topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] }@ @@ -127,42 +178,10 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so #include -void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer) -{ - struct @(uorb_struct) *topic = static_cast(in); - -@{ - -serialize_expr = " serialize_{type}(&topic->{field}, buffer);" -serialize_array_expr = " serialize_{type}(&topic->{field}[{index}], buffer);" - -buffer_memcpy_expr = " memcpy(((char *)buffer->data) + buffer->next, &topic->{field}, sizeof(topic->{field}));" -buffer_next_expr = " buffer->next += sizeof(topic->{field});" - -for each_field in sorted_fields: - if not each_field.is_header: - - # call appropriate serialize functions for fields that are orb messages - if bare_name(each_field.type) in msgtypes_composite: - - if each_field.is_array: - for i in range(each_field.array_len): - print(serialize_array_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name, index=i)) - - else: - print(serialize_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name)) - - # copy primitive fields - else: - print(buffer_memcpy_expr.format(field=each_field.name)) - print(buffer_next_expr.format(field=each_field.name)) - -}@ -} - @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );"; @[for multi_topic in topics]@ -ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields); +ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), + __orb_@(topic_name)_fields); @[end for] diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 9196edbc4d..a165f51ad5 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -13,10 +13,12 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - md5sum (String) MD5Sum of the .msg specification +@# - search_path (dict) search paths for genmsg +@# - topics (List of String) multi-topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2016 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 @@ -107,9 +109,6 @@ type_map = { 'float64': 'double', 'bool': 'bool', 'char': 'char', - 'fence_vertex': 'fence_vertex', - 'position_setpoint': 'position_setpoint', - 'esc_report': 'esc_report' } msgtype_size_map = { @@ -125,9 +124,6 @@ msgtype_size_map = { 'float64': 8, 'bool': 1, 'char': 1, - 'fence_vertex': 8, - 'position_setpoint': 104, - 'esc_report': 36 } def bare_name(msg_type): @@ -139,7 +135,62 @@ def bare_name(msg_type): return bare.split('[')[0] def sizeof_field_type(field): - return msgtype_size_map[bare_name(field.type)] + bare_name_str = bare_name(field.type) + if bare_name_str in msgtype_size_map: + return msgtype_size_map[bare_name_str] + return 0 # this is for non-builtin types: sort them at the end + +def get_children_fields(base_type): + (package, name) = genmsg.names.package_resource_name(base_type) + tmp_msg_context = genmsg.msg_loader.MsgContext.create_default() + spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path) + sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True) + return sorted_fields + +# Add padding fields before the embedded types, at the end and calculate the +# struct size +def add_padding_bytes(fields): + struct_size = 8 # account for the timestamp + align_to = 8 # this is always 8, because of the 64bit timestamp + i = 0 + padding_idx = 0 + while i < len(fields): + field = fields[i] + if not field.is_header: + a_pos = field.type.find('[') + array_size = 1 + if field.is_array: + array_size = field.array_len + if field.is_builtin: + field.sizeof_field_type = sizeof_field_type(field) + else: + # embedded type: may need to add padding + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes != align_to: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.insert(i, padding_field) + i += 1 + children_fields = get_children_fields(field.base_type) + field.sizeof_field_type, unused = add_padding_bytes(children_fields) + struct_size += field.sizeof_field_type * array_size + i += 1 + + # add padding at the end (necessary for embedded types) + num_padding_bytes = align_to - (struct_size % align_to) + if num_padding_bytes == align_to: + num_padding_bytes = 0 + else: + padding_field = genmsg.Field('_padding'+str(padding_idx), + 'uint8['+str(num_padding_bytes)+']') + padding_idx += 1 + padding_field.sizeof_field_type = 1 + struct_size += num_padding_bytes + fields.append(padding_field) + return (struct_size, num_padding_bytes) # Function to print a standard ros type def print_field_def(field): @@ -165,18 +216,23 @@ def print_field_def(field): # need to add _t: int8 --> int8_t type_px4 = type_map[type_name] else: - raise Exception("Type {0} not supported, add to to template file!".format(type_name)) + type_px4 = type_name + + comment = '' + if field.name.startswith('_padding'): + comment = ' // required for logger' - print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) + print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name, + array_size, comment)) def print_parsed_fields(): - # sort fields + # sort fields (using a stable sort) sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) + struct_size, padding_end_size = add_padding_bytes(sorted_fields) # loop over all fields and print the type and name for field in sorted_fields: if (not field.is_header): print_field_def(field) - }@ #ifdef __cplusplus @@ -205,8 +261,6 @@ for constant in spec.constants: #endif }; -void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer); - /* register this as object request broker structure */ @[for multi_topic in topics]@ ORB_DECLARE(@multi_topic); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index a323b8407b..9ba2543ead 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -494,43 +494,21 @@ void Logger::run() for (LoggerSubscription &sub : _subscriptions) { /* each message consists of a header followed by an orb data object - * The size of the data object is given by orb_metadata.o_size */ - size_t padded_msg_size = sub.metadata->o_size; - uint8_t orb_msg_padded[padded_msg_size]; + size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size; + //TODO: use sub.metadata->o_size_no_padding + uint8_t buffer[msg_size]; /* if this topic has been updated, copy the new data into the message buffer * and write a message to the log */ - //orb_check(sub.fd, &updated); // check whether a non-multi topic has been updated - /* this works for both single and multi-instances */ for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { - if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], &orb_msg_padded, &sub.time_tried_subscribe)) { + if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s), + &sub.time_tried_subscribe)) { - struct orb_output_buffer output_buffer = {}; - uint8_t buffer[sizeof(message_data_header_s) + padded_msg_size]; - output_buffer.data = &buffer; - output_buffer.next = sizeof(message_data_header_s); - - sub.metadata->serialize(&orb_msg_padded, &output_buffer); - size_t msg_size = output_buffer.next; - - //uint64_t timestamp; - //memcpy(×tamp, buffer + sizeof(message_data_header_s), sizeof(timestamp)); - //warnx("topic: %s, instance: %d, timestamp: %llu", - // sub.metadata->o_name, instance, timestamp); - - /* copy the current topic data into the buffer after the header */ - //orb_copy(sub.metadata, sub.fd, buffer + sizeof(message_data_header_s)); - - /* fill the message header struct in-place at the front of the buffer, - * accessing the unaligned (packed) structure properly - */ message_data_header_s *header = reinterpret_cast(buffer); header->msg_type = static_cast(MessageType::DATA); - /* the ORB topic data object has 2 unused trailing bytes? */ - //header->msg_size = static_cast(msg_size - 3); - header->msg_size = msg_size; + header->msg_size = static_cast(msg_size - 3); header->msg_id = msg_id; header->multi_id = 0x80 + instance; // Non multi, active diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 1b06a2ad89..fba37a57af 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -46,21 +46,14 @@ // Hack until everything is using this header #include -struct orb_output_buffer { - void *data; - size_t next; -}; - -typedef void (*func_ptr)(void *in, struct orb_output_buffer *out); - /** * Object metadata. */ struct orb_metadata { const char *o_name; /**< unique object name */ const size_t o_size; /**< object size */ - func_ptr serialize; /**< serialization function for this orb topic */ - const char *o_fields; /**< semicolon separated list of fields */ + const size_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) */ }; typedef const struct orb_metadata *orb_id_t; @@ -119,14 +112,14 @@ enum ORB_PRIO { * * @param _name The name of the topic. * @param _struct The structure the topic provides. - * @param _func The pointer to a function that serializes this topic + * @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" */ -#define ORB_DEFINE(_name, _struct, _func, _fields) \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields) \ const struct orb_metadata __orb_##_name = { \ #_name, \ sizeof(_struct), \ - _func, \ + _size_no_padding, \ _fields \ }; struct hack diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index 6f5012cec3..5962b62d09 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -41,8 +41,8 @@ struct orb_test { int val; hrt_abstime time; }; -ORB_DEFINE(orb_test, struct orb_test, nullptr, "ORB_TEST:int val;hrt_abstime time;"); -ORB_DEFINE(orb_multitest, struct orb_test, nullptr, "ORB_MULTITEST:int val;hrt_abstime time;"); +ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;"); +ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;"); struct orb_test_medium { @@ -50,9 +50,10 @@ struct orb_test_medium { hrt_abstime time; char junk[64]; }; -ORB_DEFINE(orb_test_medium, struct orb_test_medium, nullptr, "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;"); -ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, nullptr, - "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); struct orb_test_large { @@ -60,7 +61,8 @@ struct orb_test_large { hrt_abstime time; char junk[512]; }; -ORB_DEFINE(orb_test_large, struct orb_test_large, nullptr, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;"); +ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large), + "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;"); namespace uORBTest