From 3db17a04fc9dd7e035cda5a1a952bd04ee3d2954 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 23 Mar 2018 11:43:19 -0400
Subject: [PATCH] uORB generate message print functions

---
 msg/CMakeLists.txt                         |  4 +-
 msg/templates/uorb/msg.cpp.template        | 12 ++++
 msg/templates/uorb/msg.h.template          |  4 ++
 msg/tools/px_generate_uorb_topic_helper.py | 78 ++++++++++++++++++++++
 4 files changed, 96 insertions(+), 2 deletions(-)

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index c88372dad3..63abeaa5ef 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -165,7 +165,7 @@ add_custom_command(OUTPUT ${uorb_headers}
 		-e templates/uorb
 		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers
 		-q
-	DEPENDS ${msg_files}
+	DEPENDS ${msg_files} templates/uorb/msg.h.template
 	COMMENT "Generating uORB topic headers"
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	VERBATIM
@@ -182,7 +182,7 @@ add_custom_command(OUTPUT ${uorb_sources}
 		-e templates/uorb
 		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources
 		-q
-	DEPENDS ${msg_files}
+	DEPENDS ${msg_files} templates/uorb/msg.cpp.template
 	COMMENT "Generating uORB topic sources"
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	VERBATIM
diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 3437e5c109..3b139b1288 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -64,6 +64,9 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
+#include <cinttypes>
+#include <cstdio>
+#include <px4_defines.h>
 #include <uORB/topics/@(topic_name).h>
 
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
@@ -73,3 +76,12 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
 @[for multi_topic in topics]@
 ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields);
 @[end for]
+
+void print_message(const @uorb_struct& message)
+{
+    printf(" @(uorb_struct)\n");
+	printf("\ttimestamp: %" PRIu64 "\n", message.timestamp);
+@[for field in sorted_fields]@
+	@( px4_printf(field) )
+@[end for]
+}
\ No newline at end of file
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 1d1c74271e..1ea312451b 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -133,3 +133,7 @@ for constant in spec.constants:
 @[for multi_topic in topics]@
 ORB_DECLARE(@multi_topic);
 @[end for]
+
+#ifdef __cplusplus
+void print_message(const @uorb_struct& message);
+#endif
diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py
index 7f55b0ee6a..00d27cb4a2 100644
--- a/msg/tools/px_generate_uorb_topic_helper.py
+++ b/msg/tools/px_generate_uorb_topic_helper.py
@@ -68,6 +68,20 @@ msgtype_size_map = {
     'char': 1,
 }
 
+type_printf_map = {
+    'int8': '%d',
+    'int16': '%d',
+    'int32': '%" PRId32 "',
+    'int64': '%" PRId64 "',
+    'uint8': '%u',
+    'uint16': '%u',
+    'uint32': '%" PRIu32 "',
+    'uint64': '%" PRIu64 "',
+    'float32': '%.3f',
+    'float64': '%.3f',
+    'bool': '%u',
+    'char': '%c',
+}
 
 def bare_name(msg_type):
     """
@@ -165,6 +179,70 @@ def convert_type(spec_type):
     return c_type
 
 
+def px4_printf(field):
+    """
+    Echo printf line
+    """
+
+    # skip padding
+    if ("_padding" in field.name):
+        return
+
+    bare_type = field.type
+    if '/' in field.type:
+        # removing prefix
+        bare_type = (bare_type.split('/'))[1]
+
+    msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
+
+    field_name = ""
+
+    if is_array:
+        c_type = "["
+
+        if msg_type in type_map:
+            p_type = type_printf_map[msg_type]
+
+        else:
+            for i in range(array_length):
+                print("printf(\"   " + field.type + " " + field.name + "[" + str(i) + "]\");")
+                print(" print_message(message." + field.name + "[" + str(i) + "]);")
+            return
+
+        for i in range(array_length):
+
+            if i > 0:
+                c_type += ", "
+                field_name += ", "
+
+            if "float32" in field.type:
+                field_name += "(double)message." + field.name + "[" + str(i) + "]"
+            else:
+                field_name += "message." + field.name + "[" + str(i) + "]"
+
+            c_type += str(p_type)
+
+        c_type += "]"
+
+    else:
+        c_type = msg_type
+        if msg_type in type_map:
+            c_type = type_printf_map[msg_type]
+
+            field_name = "message." + field.name
+
+            # cast double
+            if field.type == "float32":
+                field_name = "(double)" + field_name
+
+        else:
+            print("printf(\"  " + field.name + "\");")
+            print("\tprint_message(message."+ field.name + ");")
+            return
+
+    print("printf(\"\t" + field.name + ": " + c_type + "\\n\", " + field_name + ");" )
+
+
 def print_field_def(field):
     """
     Print the C type from a field