orb structs: add padding bytes to align the structs where necessary

This is required for the logger, we just manually add the padding bytes
what would otherwise be done by the compiler. Additionally we reorder
the fields by type, so that padding is only necessary for nested types.

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 <uORB/topics/@(topic_name).h>
 
 
-void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer)
-{
-	struct @(uorb_struct) *topic = static_cast<struct @(uorb_struct) *>(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]

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

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(&timestamp, 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<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						/* the ORB topic data object has 2 unused trailing bytes? */
-						//header->msg_size = static_cast<uint16_t>(msg_size - 3);
-						header->msg_size = msg_size;
+						header->msg_size = static_cast<uint16_t>(msg_size - 3);
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 

src/modules/uORB/uORB.h

@@ -46,21 +46,14 @@
 // Hack until everything is using this header
 #include <systemlib/visibility.h>
 
-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
 

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