diff options
Diffstat (limited to 'src/modules/uORB')
-rw-r--r-- | src/modules/uORB/uORB.cpp | 85 |
1 files changed, 75 insertions, 10 deletions
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 6f021459c..764e33179 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -56,6 +56,7 @@ #include <nuttx/arch.h> #include <nuttx/wqueue.h> #include <nuttx/clock.h> +#include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> @@ -682,9 +683,11 @@ namespace { ORBDevMaster *g_dev; +bool pubsubtest_passed = false; struct orb_test { int val; + hrt_abstime time; }; ORB_DEFINE(orb_test, struct orb_test); @@ -718,6 +721,46 @@ test_note(const char *fmt, ...) return OK; } +int pubsublatency_main(void) +{ + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + struct pollfd fds[1]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + + struct orb_test t; + + const unsigned maxruns = 10; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t); + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + } + + orb_unsubscribe(test_multi_sub); + + warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns)); + + pubsubtest_passed = true; + + return OK; +} + int test() { @@ -779,8 +822,7 @@ test() int instance0; int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - test_note("advertised"); - usleep(300000); + test_note("advertised"); int instance1; int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); @@ -795,8 +837,7 @@ test() if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) return test_fail("mult. pub0 fail"); - test_note("published"); - usleep(300000); + test_note("published"); t.val = 203; if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) @@ -805,19 +846,19 @@ test() /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) return test_fail("sub #0 copy failed: %d", errno); - if (t.val != 103) - return test_fail("sub #0 val. mismatch: %d", t.val); + if (u.val != 103) + return test_fail("sub #0 val. mismatch: %d", u.val); int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) return test_fail("sub #1 copy failed: %d", errno); - if (t.val != 203) - return test_fail("sub #1 val. mismatch: %d", t.val); + if (u.val != 203) + return test_fail("sub #1 val. mismatch: %d", u.val); /* test priorities */ int prio; @@ -833,6 +874,30 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); + /* test pub / sub latency */ + + int pubsub_task = task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (main_t)&pubsublatency_main, + nullptr); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 303; + t.time = hrt_absolute_time(); + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 timing fail"); + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + return test_note("PASS"); } |