WIP logger serialization

msg/templates/uorb/msg.cpp.template

@@ -57,7 +57,8 @@ uorb_struct = '%s_s'%spec.short_name
 uorb_pack_func = 'pack_%s'%spec.short_name
 topic_name = spec.short_name
 
-type_map = {'int8': 'int8_t',
+type_map = {
+  'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -71,23 +72,28 @@ type_map = {'int8': 'int8_t',
   'char': 'char',
   'fence_vertex': 'fence_vertex',
   'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'}
+  'esc_report': 'esc_report'
+}
 
-msgtype_size_map = {'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
+msgtype_size_map = {
+  'int8': 1,
+  'int16': 2,
+  'int32': 4,
+  'int64': 8,
+  'uint8': 1,
+  'uint16': 2,
+  'uint32': 4,
+  'uint64': 8,
+  'float32': 4,
+  'float64': 8,
   'bool': 1,
   'char': 1,
   'fence_vertex': 8,
   'position_setpoint': 104,
-  'esc_report': 36}
+  'esc_report': 36
+}
+
+msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex']
 
 def convert_type(spec_type):
     bare_type = spec_type
@@ -112,7 +118,7 @@ def bare_name(msg_type):
 def sizeof_field_type(field):
     return msgtype_size_map[bare_name(field.type)]
 
-sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
@@ -120,9 +126,43 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
 #include <drivers/drv_orb_dev.h>
 #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, __orb_@(topic_name)_fields);
+ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields);
 @[end for]

msg/templates/uorb/msg.h.template

@@ -68,6 +68,7 @@ topic_name = spec.short_name
 #else
 #include <string.h>
 #endif
+
 #include <uORB/uORB.h>
 
 @##############################
@@ -93,7 +94,8 @@ for field in spec.parsed_fields():
 @# Main struct of message
 @##############################
 @{
-type_map = {'int8': 'int8_t',
+type_map = {
+  'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -111,16 +113,16 @@ type_map = {'int8': 'int8_t',
 }
 
 msgtype_size_map = {
-  'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
+  'int8': 1,
+  'int16': 2,
+  'int32': 4,
+  'int64': 8,
+  'uint8': 1,
+  'uint16': 2,
+  'uint32': 4,
+  'uint64': 8,
+  'float32': 4,
+  'float64': 8,
   'bool': 1,
   'char': 1,
   'fence_vertex': 8,
@@ -128,33 +130,6 @@ msgtype_size_map = {
   'esc_report': 36
 }
 
-def bare_name(msg_type):
-    bare = msg_type
-    if '/' in msg_type:
-        # removing prefix
-        bare = (msg_type.split('/'))[1]
-    # removing suffix
-    return bare.split('[')[0]
-
-def sizeof_field_type(field):
-    return msgtype_size_map[bare_name(field.type)]
-
-msgtype_size_map = {'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
-  'bool': 1,
-  'char': 1,
-  'fence_vertex': 8,
-  'position_setpoint': 104,
-  'esc_report': 36}
-
 def bare_name(msg_type):
     bare = msg_type
     if '/' in msg_type:
@@ -196,7 +171,7 @@ def print_field_def(field):
 
 def print_parsed_fields():
   # sort fields
-  sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+  sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
   # loop over all fields and print the type and name
   for field in sorted_fields:
     if (not field.is_header):
@@ -204,7 +179,6 @@ def print_parsed_fields():
 
 }@
 
-#pragma pack(push, 1)
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -230,11 +204,8 @@ for constant in spec.constants:
 }
 #endif
 };
-#pragma pack(pop)
 
-/**
- * @@}
- */
+void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer);
 
 /* register this as object request broker structure */
 @[for multi_topic in topics]@