diff --git a/src/systemcmds/topic_listener/generate_listener.py b/src/systemcmds/topic_listener/generate_listener.py
index 8a8b95beb7..54cb564ba4 100755
--- a/src/systemcmds/topic_listener/generate_listener.py
+++ b/src/systemcmds/topic_listener/generate_listener.py
@@ -123,7 +123,7 @@ for index, (m, t) in enumerate(zip(messages, topics)):
 		print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
 	else:
 		print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t))
-	print("\t\tlistener<%s_s>(ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
+	print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
 
 print("\t} else {")
 print("\t\t PX4_INFO_RAW(\" Topic did not match any known topics\\n\");")
diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp
index 030ded3cb9..99efcada91 100644
--- a/src/systemcmds/topic_listener/listener_main.cpp
+++ b/src/systemcmds/topic_listener/listener_main.cpp
@@ -47,6 +47,54 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]);
 
 static void usage();
 
+void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance,
+	      unsigned topic_interval)
+{
+	if (orb_exists(id, topic_instance) != 0) {
+		PX4_INFO_RAW("never published\n");
+		return;
+	}
+
+	int sub = orb_subscribe_multi(id, topic_instance);
+	orb_set_interval(sub, topic_interval);
+
+	bool updated = false;
+	unsigned i = 0;
+	hrt_abstime start_time = hrt_absolute_time();
+
+	while (i < num_msgs) {
+		orb_check(sub, &updated);
+
+		if (i == 0) {
+			updated = true;
+
+		} else {
+			usleep(500);
+		}
+
+		if (updated) {
+			start_time = hrt_absolute_time();
+			i++;
+
+			PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
+
+			int ret = cb(id, sub);
+
+			if (ret != PX4_OK) {
+				PX4_ERR("listener callback failed (%i)", ret);
+			}
+
+		} else {
+			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
+				PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n");
+				break;
+			}
+		}
+	}
+
+	orb_unsubscribe(sub);
+}
+
 int listener_main(int argc, char *argv[])
 {
 	if (argc <= 1) {
diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp
index 8760ca2b3d..7a4c17a684 100644
--- a/src/systemcmds/topic_listener/topic_listener.hpp
+++ b/src/systemcmds/topic_listener/topic_listener.hpp
@@ -51,53 +51,21 @@
 #include <stdlib.h>
 #include <inttypes.h>
 
+typedef int(*listener_print_topic_cb)(const orb_id_t &orb_id, int subscription);
+
 template <typename T>
-void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
+int listener_print_topic(const orb_id_t &orb_id, int subscription)
 {
-	if (orb_exists(id, topic_instance) != 0) {
-		PX4_INFO_RAW("never published\n");
-		return;
+	T container;
+
+	int ret = orb_copy(orb_id, subscription, &container);
+
+	if (ret == PX4_OK) {
+		print_message(container);
 	}
 
-	int sub = orb_subscribe_multi(id, topic_instance);
-	orb_set_interval(sub, topic_interval);
-
-	bool updated = false;
-	unsigned i = 0;
-	hrt_abstime start_time = hrt_absolute_time();
-
-	while (i < num_msgs) {
-		orb_check(sub, &updated);
-
-		if (i == 0) {
-			updated = true;
-
-		} else {
-			usleep(500);
-		}
-
-		if (updated) {
-			start_time = hrt_absolute_time();
-			i++;
-
-			PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
-
-			T container;
-
-			if (orb_copy(id, sub, &container) == PX4_OK) {
-				print_message(container);
-
-			} else {
-				PX4_ERR("orb_copy failed");
-			}
-
-		} else {
-			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
-				PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n");
-				break;
-			}
-		}
-	}
-
-	orb_unsubscribe(sub);
+	return ret;
 }
+
+void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance,
+	      unsigned topic_interval);