From ba5ef28f231ed62c9bea2d9dfcc71f709e539d30 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 30 Oct 2020 10:45:16 -0400
Subject: [PATCH] uORB_tests: add uORB::SubscriptionMultiArray tests

---
 .../uORB/uORB_tests/uORBTest_UnitTest.cpp     | 104 +++++++++++++++++-
 .../uORB/uORB_tests/uORBTest_UnitTest.hpp     |   2 +
 2 files changed, 105 insertions(+), 1 deletion(-)

diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index ce6b7d5613..f62a21898b 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -39,6 +39,8 @@
 #include <errno.h>
 #include <math.h>
 #include <lib/cdev/CDev.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 
 uORBTest::UnitTest &uORBTest::UnitTest::instance()
 {
@@ -178,6 +180,12 @@ int uORBTest::UnitTest::test()
 		return ret;
 	}
 
+	ret = test_SubscriptionMulti();
+
+	if (ret != OK) {
+		return ret;
+	}
+
 	ret = test_multi();
 
 	if (ret != OK) {
@@ -741,7 +749,101 @@ int uORBTest::UnitTest::test_wrap_around()
 	orb_unadvertise(ptopic);
 	orb_unsubscribe(sfd);
 
-	return test_note("PASS orb queuing");
+	return test_note("PASS orb wrap around");
+}
+
+int uORBTest::UnitTest::test_SubscriptionMulti()
+{
+
+	uORB::PublicationMulti<orb_test_s> orb_test_pub_multi[ORB_MULTI_MAX_INSTANCES] {
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+		ORB_ID::orb_test,
+	};
+
+	uORB::SubscriptionMultiArray<orb_test_s> orb_test_sub_multi_array{ORB_ID::orb_test};
+
+	// verify not advertised yet
+	for (auto &sub : orb_test_sub_multi_array) {
+		if (sub.advertised()) {
+			return test_fail("sub is advertised");
+		}
+	}
+
+	// advertise one at a time and verify instance
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		orb_test_pub_multi[i].advertise();
+
+		if (!orb_test_sub_multi_array[i].advertised()) {
+			return test_fail("sub %d advertise failed", i);
+		}
+
+		if (orb_test_sub_multi_array.advertised_count() != i + 1) {
+			return test_fail("SubscriptionMultiArray advertised_count() return %d, should be %d",
+					 orb_test_sub_multi_array.advertised_count(), i);
+		}
+
+		// verify instance numbering
+		if (orb_test_sub_multi_array[i].get_instance() != i) {
+			return test_fail("sub %d doesn't match instance %d", i, orb_test_sub_multi_array[i].get_instance());
+		}
+	}
+
+	// publish one at a time and verify
+	for (int t = 0; t < 2; t++) {
+		for (int i = 0; i < 10; i++) {
+			for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
+				orb_test_s orb_test_pub{};
+				orb_test_pub.val = i * instance + t;
+				orb_test_pub.timestamp = hrt_absolute_time();
+				orb_test_pub_multi[instance].publish(orb_test_pub);
+			}
+
+			int sub_instance = 0;
+
+			for (auto &sub : orb_test_sub_multi_array) {
+
+				if (!sub.updated()) {
+					return test_fail("sub %d not updated", sub_instance);
+
+				} else {
+					const unsigned last_gen = sub.get_last_generation();
+
+					orb_test_s orb_test_copy{};
+
+					if (!sub.copy(&orb_test_copy)) {
+						return test_fail("sub %d copy failed", sub_instance);
+
+					} else {
+						if (orb_test_copy.val != (i * sub_instance + t)) {
+							return test_fail("sub %d invalid value %d", sub_instance, orb_test_copy.val);
+						}
+
+						if (sub.get_last_generation() != last_gen + 1) {
+							//return test_fail("sub %d generation should be %d + 1, but it's %d", sub_instance, last_gen, sub.get_last_generation());
+							PX4_ERR("sub %d generation should be %d + 1, but it's %d", sub_instance, last_gen, sub.get_last_generation());
+						}
+					}
+				}
+
+				sub_instance++;
+			}
+		}
+
+		// force unsubscribe all, then repeat
+		for (auto &sub : orb_test_sub_multi_array) {
+			sub.unsubscribe();
+		}
+	}
+
+	return test_note("PASS orb SubscriptionMulti");
 }
 
 int uORBTest::UnitTest::test_queue()
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 6e3026429b..7e21fe1ffc 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -105,6 +105,8 @@ private:
 
 	int test_wrap_around();
 
+	int test_SubscriptionMulti();
+
 	/* queuing tests */
 	int test_queue();
 	static int pub_test_queue_entry(int argc, char *argv[]);