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]