Merge pull request #4279 from bkueng/fix_listener_cmd

Fix listener cmd

Tools/generate_listener.py

@@ -106,6 +106,7 @@ print("""
  * Tool for listening to topics when running flight stack on linux.
  */
 
+#include <drivers/drv_hrt.h>
 #include <px4_middleware.h>
 #include <px4_app.h>
 #include <px4_config.h>
@@ -125,6 +126,14 @@ print("""
 #define PRId64 "lld"
 #endif
 
+static bool check_timeout(const hrt_abstime& time) {
+    if (hrt_elapsed_time(&time) > 2*1000*1000) {
+		printf("Waited for 2 seconds without a message. Giving up.\\n");
+        return true;
+    }
+    return false;
+}
+
 """)
 for m in messages:
 	print("#include <uORB/topics/%s.h>" % m)
@@ -155,12 +164,14 @@ for index,m in enumerate(messages[1:]):
 	print("\t\tmemset(&container, 0, sizeof(container));")
 	print("\t\tbool updated;")
 	print("\t\tunsigned i = 0;")
+	print("\t\thrt_abstime start_time = hrt_absolute_time();")
 	print("\t\twhile(i < num_msgs) {")
 	print("\t\t\torb_check(sub,&updated);")
 	print("\t\t\tif (i == 0) { updated = true; } else { usleep(500); }")
-	print("\t\t\ti++;")
 	print("\t\t\tif (updated) {")
-	print("\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m)
+	print("\t\t\tstart_time = hrt_absolute_time();")
+	print("\t\t\ti++;")
+	print("\t\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m)
 	print("\t\t\torb_copy(ID,sub,&container);")
 	for item in message_elements[index+1]:
 		if item[0] == "float":
@@ -203,6 +214,10 @@ for index,m in enumerate(messages[1:]):
 			print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1]))
 		elif item[0] == "bool":
 			print("\t\t\tprintf(\"%s: %%s\\n\",container.%s ? \"True\" : \"False\");" % (item[1], item[1]))
+	print("\t\t\t} else {")
+	print("\t\t\t\tif (check_timeout(start_time)) {")
+	print("\t\t\t\t\tbreak;")
+	print("\t\t\t\t}")
 	print("\t\t\t}")
 	print("\t\t}")
 print("\t} else {")