From 8fa18f412a4437312fbe46ccfaed16b6dddfdc43 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 16 Apr 2016 10:16:50 +0200
Subject: [PATCH] uorb: add unit test that currently fails (simulation of a
 queue with a single topic)

This fails both on NuttX & Linux
---
 src/modules/uORB/uORBTest_UnitTest.cpp | 118 ++++++++++++++++++++++++-
 src/modules/uORB/uORBTest_UnitTest.hpp |   9 +-
 2 files changed, 125 insertions(+), 2 deletions(-)

diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp
index 9a0b22519d..ecb5f06ba6 100644
--- a/src/modules/uORB/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORBTest_UnitTest.cpp
@@ -158,7 +158,7 @@ int uORBTest::UnitTest::test()
 		return ret;
 	}
 
-	return OK;
+	return test_multi2();
 }
 
 
@@ -318,9 +318,125 @@ int uORBTest::UnitTest::test_multi()
 		return test_fail("latency test failed");
 	}
 
+	orb_unsubscribe(sfd0);
+	orb_unsubscribe(sfd1);
+
 	return test_note("PASS multi-topic test");
 }
 
+
+
+int uORBTest::UnitTest::pub_test_multi2_entry(char *const argv[])
+{
+	uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+	return t.pub_test_multi2_main();
+}
+
+int uORBTest::UnitTest::pub_test_multi2_main()
+{
+	int data_next_idx = 0;
+	const int num_instances = 3;
+	orb_advert_t orb_pub[num_instances];
+	struct orb_test_medium data_topic;
+
+	for (int i = 0; i < num_instances; ++i) {
+		orb_advert_t &pub = orb_pub[i];
+		int idx = i;
+//		PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
+		pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);
+
+		if (idx != i) {
+			_thread_should_exit = true;
+			PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx);
+			return -1;
+		}
+	}
+
+	usleep(100 * 1000);
+
+	int message_counter = 0, num_messages = 100 * num_instances;
+
+	while (message_counter++ < num_messages) {
+		usleep(2); //make sure the timestamps are different
+		orb_advert_t &pub = orb_pub[data_next_idx];
+
+		data_topic.time = hrt_absolute_time();
+		data_topic.val = data_next_idx;
+
+		orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
+//		PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time);
+
+		data_next_idx = (data_next_idx + 1) % num_instances;
+
+		if (data_next_idx == 0) {
+			usleep(10 * 1000);
+		}
+	}
+
+	usleep(100 * 1000);
+	_thread_should_exit = true;
+
+	return 0;
+}
+
+int uORBTest::UnitTest::test_multi2()
+{
+
+	//test: first subscribe, then advertise
+
+	_thread_should_exit = false;
+	const int num_instances = 3;
+	int orb_data_fd[num_instances];
+	int orb_data_next = 0;
+
+	for (int i = 0; i < num_instances; ++i) {
+//		PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
+		orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
+	}
+
+	char *const args[1] = { NULL };
+	int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 5,
+					     1500,
+					     (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
+					     args);
+
+	if (pubsub_task < 0) {
+		return test_fail("failed launching task");
+	}
+
+	hrt_abstime last_time = 0;
+
+	while (!_thread_should_exit) {
+
+		bool updated = false;
+		int orb_data_cur_fd = orb_data_fd[orb_data_next];
+		orb_check(orb_data_cur_fd, &updated);
+
+		if (updated) {
+			struct orb_test_medium msg;
+			orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
+			usleep(1000);
+
+			if (last_time >= msg.time && last_time != 0) {
+				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
+			}
+
+			last_time = msg.time;
+
+//			PX4_WARN("      got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
+			orb_data_next = (orb_data_next + 1) % num_instances;
+		}
+	}
+
+	for (int i = 0; i < num_instances; ++i) {
+		orb_unsubscribe(orb_data_fd[i]);
+	}
+
+	return test_note("PASS multi-topic 2 test (queue simulation)");
+}
+
 int uORBTest::UnitTest::test_multi_reversed()
 {
 	test_note("try multi-topic support subscribing before publishing");
diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp
index 5f8d087ac1..2485a9a120 100644
--- a/src/modules/uORB/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORBTest_UnitTest.hpp
@@ -50,6 +50,7 @@ struct orb_test_medium {
 	char junk[64];
 };
 ORB_DEFINE(orb_test_medium, struct orb_test_medium);
+ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium);
 
 struct orb_test_large {
 	int val;
@@ -82,13 +83,19 @@ private:
 	UnitTest(const uORBTest::UnitTest &) {};
 	static int pubsubtest_threadEntry(char *const argv[]);
 	int pubsublatency_main(void);
-	//
+
+	static int pub_test_multi2_entry(char *const argv[]);
+	int pub_test_multi2_main();
+
+	volatile bool _thread_should_exit;
+
 	bool pubsubtest_passed;
 	bool pubsubtest_print;
 	int pubsubtest_res = OK;
 
 	int test_single();
 	int test_multi();
+	int test_multi2();
 	int test_multi_reversed();
 
 	int test_fail(const char *fmt, ...);