diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py
index 1b55528372..7849905a94 100755
--- a/Tools/generate_listener.py
+++ b/Tools/generate_listener.py
@@ -89,24 +89,28 @@ print("""
  ****************************************************************************/
 
 /**
- * @file topic_listener.cpp, autogenerated by Tools/generate_listener.py
+ * @file topic_listener.cpp
+ *
+ * Autogenerated by Tools/generate_listener.py
  *
  * Tool for listening to topics when running flight stack on linux.
  */
+
 #include <px4_middleware.h>
 #include <px4_app.h>
 #include <px4_config.h>
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <sstream>
-#include <vector>
-#include <cstring>
 #include <uORB/uORB.h>
 #include <string.h>
 #include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
 #define __STDC_FORMAT_MACROS
 #include <inttypes.h>
+
+#ifndef PRIu64
+#define PRIu64 "ull"
+#endif
+
 """)
 for m in messages:
 	print("#include <uORB/topics/%s.h>" % m)
@@ -123,7 +127,7 @@ int listener_main(int argc, char *argv[]) {
 		return 1;
 	}
 """)
-print("\tuint32_t num_msgs = (uint32_t)std::stoi(argv[2],NULL,10);")
+print("\tunsigned num_msgs = atoi(argv[2]);")
 print("\tif(strncmp(argv[1],\"%s\",50)== 0) {" % messages[0])
 print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0])
 print("\t\tID = ORB_ID(%s);" % messages[0])
@@ -140,6 +144,7 @@ for index,m in enumerate(messages[1:]):
 	print("\t\t\torb_check(sub,&updated);")
 	print("\t\t\tupdated = true;")
 	print("\t\t\tif(updated) {")
+	print("\t\tprintf(\"\\nTOPIC: %s\");" % m)
 	print("\t\t\torb_copy(ID,sub,&container);")
 	for item in message_elements[index+1]:
 		if item[0] == "float":
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index fec5f06370..8788bbeb51 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -53,6 +53,7 @@ set(config_module_list
 	systemcmds/pwm
 	systemcmds/esc_calib
 	systemcmds/reboot
+	systemcmds/topic_listener
 	systemcmds/top
 	systemcmds/config
 	systemcmds/nshterm
diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt
index 2f720757ac..eb3071f41b 100644
--- a/src/systemcmds/topic_listener/CMakeLists.txt
+++ b/src/systemcmds/topic_listener/CMakeLists.txt
@@ -36,7 +36,9 @@ add_custom_command(OUTPUT topic_listener.cpp
                 )
 
 add_custom_target(generate_topic_listener
-	DEPENDS topic_listener.cpp)
+	DEPENDS
+		topic_listener.cpp
+		${CMAKE_SOURCE_DIR}/Tools/generate_listener.py)
 
 px4_add_module(
 	MODULE systemcmds__topic_listener