diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp index 78b2f0e1e0..da8f859422 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -36,6 +36,8 @@ #include #include #include +#include +#include uORBTest::UnitTest &uORBTest::UnitTest::instance() { @@ -164,7 +166,19 @@ int uORBTest::UnitTest::test() return ret; } - return test_multi2(); + ret = test_multi2(); + + if (ret != OK) { + return ret; + } + + ret = test_queue(); + + if (ret != OK) { + return ret; + } + + return test_queue_poll_notify(); } int uORBTest::UnitTest::test_unadvertise() @@ -557,6 +571,226 @@ int uORBTest::UnitTest::test_multi_reversed() return test_note("PASS multi-topic reversed"); } +int uORBTest::UnitTest::test_queue() +{ + test_note("Testing orb queuing"); + + struct orb_test_medium t, u; + int sfd; + orb_advert_t ptopic; + bool updated; + + sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); + + if (sfd < 0) { + return test_fail("subscribe failed: %d", errno); + } + + + const int queue_size = 11; + t.val = 0; + ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); + + if (ptopic == nullptr) { + return test_fail("advertise failed: %d", errno); + } + + orb_check(sfd, &updated); + + if (!updated) { + return test_fail("update flag not set"); + } + + if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) { + return test_fail("copy(1) failed: %d", errno); + } + + if (u.val != t.val) { + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + } + + orb_check(sfd, &updated); + + if (updated) { + return test_fail("spurious updated flag"); + } + +#define CHECK_UPDATED(element) \ + orb_check(sfd, &updated); \ + if (!updated) { \ + return test_fail("update flag not set, element %i", element); \ + } +#define CHECK_NOT_UPDATED(element) \ + orb_check(sfd, &updated); \ + if (updated) { \ + return test_fail("update flag set, element %i", element); \ + } +#define CHECK_COPY(i_got, i_correct) \ + orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ + if (i_got != i_correct) { \ + return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ + } + + //no messages in the queue anymore + + test_note(" Testing to write some elements..."); + + for (int i = 0; i < queue_size - 2; ++i) { + t.val = i; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + } + + for (int i = 0; i < queue_size - 2; ++i) { + CHECK_UPDATED(i); + CHECK_COPY(u.val, i); + } + + CHECK_NOT_UPDATED(queue_size); + + test_note(" Testing overflow..."); + int overflow_by = 3; + + for (int i = 0; i < queue_size + overflow_by; ++i) { + t.val = i; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + } + + for (int i = 0; i < queue_size; ++i) { + CHECK_UPDATED(i); + CHECK_COPY(u.val, i + overflow_by); + } + + CHECK_NOT_UPDATED(queue_size); + + test_note(" Testing underflow..."); + + for (int i = 0; i < queue_size; ++i) { + CHECK_NOT_UPDATED(i); + CHECK_COPY(u.val, queue_size + overflow_by - 1); + } + + t.val = 943; + orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + CHECK_UPDATED(-1); + CHECK_COPY(u.val, t.val); + +#undef CHECK_COPY +#undef CHECK_UPDATED +#undef CHECK_NOT_UPDATED + + orb_unadvertise(ptopic); + + return test_note("PASS orb queuing"); +} + + +int uORBTest::UnitTest::pub_test_queue_entry(char *const argv[]) +{ + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.pub_test_queue_main(); +} + +int uORBTest::UnitTest::pub_test_queue_main() +{ + struct orb_test_medium t; + orb_advert_t ptopic; + const int queue_size = 50; + t.val = 0; + + if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + _thread_should_exit = true; + return test_fail("advertise failed: %d", errno); + } + + int message_counter = 0, num_messages = 20 * queue_size; + ++t.val; + + while (message_counter < num_messages) { + + //simulate burst + int burst_counter = 0; + + while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned + orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t); + ++t.val; + } + + message_counter += burst_counter; + usleep(20 * 1000); //give subscriber a chance to catch up + } + + _num_messages_sent = t.val; + usleep(100 * 1000); + _thread_should_exit = true; + orb_unadvertise(ptopic); + + return 0; +} + +int uORBTest::UnitTest::test_queue_poll_notify() +{ + test_note("Testing orb queuing (poll & notify)"); + + struct orb_test_medium t; + int sfd; + + if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { + return test_fail("subscribe failed: %d", errno); + } + + _thread_should_exit = false; + + char *const args[1] = { NULL }; + int pubsub_task = px4_task_spawn_cmd("uorb_test_queue", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN + 5, + 1500, + (px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry, + args); + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + + int next_expected_val = 0; + px4_pollfd_struct_t fds[1]; + fds[0].fd = sfd; + fds[0].events = POLLIN; + + while (!_thread_should_exit) { + + int poll_ret = px4_poll(fds, 1, 500); + + if (poll_ret == 0) { + if (_thread_should_exit) { + break; + } + + return test_fail("poll timeout"); + + } else if (poll_ret < 0) { + return test_fail("poll error (%d, %d)", poll_ret, errno); + } + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t); + + if (next_expected_val != t.val) { + return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val); + } + + ++next_expected_val; + } + } + if (_num_messages_sent != next_expected_val) { + return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)", + _num_messages_sent, next_expected_val); + } + + return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val); +} + + int uORBTest::UnitTest::test_fail(const char *fmt, ...) { va_list ap; diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index cf81d36e23..8016b9c8a2 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -54,7 +54,10 @@ ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium), "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;"); ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium), "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); - +ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); +ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium), + "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); struct orb_test_large { int val; @@ -98,13 +101,23 @@ private: bool pubsubtest_print; int pubsubtest_res = OK; - int test_unadvertise(); orb_advert_t _pfd[4]; ///< used for test_multi and test_multi_reversed int test_single(); + + /* These 3 depend on each other and must be called in this order */ int test_multi(); - int test_multi2(); int test_multi_reversed(); + int test_unadvertise(); + + int test_multi2(); + + /* queuing tests */ + int test_queue(); + static int pub_test_queue_entry(char *const argv[]); + int pub_test_queue_main(); + int test_queue_poll_notify(); + volatile int _num_messages_sent = 0; int test_fail(const char *fmt, ...); int test_note(const char *fmt, ...);