35 #include "../uORBCommon.hpp" 36 #include <px4_platform_common/px4_config.h> 37 #include <px4_platform_common/time.h> 48 "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
50 "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
52 "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
54 "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
57 "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
68 float latency_integral = 0.0f;
71 px4_pollfd_struct_t fds[3];
84 fds[0].fd = test_multi_sub;
85 fds[0].events = POLLIN;
86 fds[1].fd = test_multi_sub_medium;
87 fds[1].events = POLLIN;
88 fds[2].fd = test_multi_sub_large;
89 fds[2].events = POLLIN;
91 const unsigned maxruns = 1000;
92 unsigned timingsgroup = 0;
93 int current_value = t.
val;
97 unsigned *timings =
new unsigned[maxruns];
98 unsigned timing_min = 9999999, timing_max = 0;
100 for (
unsigned i = 0; i < maxruns; i++) {
102 int pret =
px4_poll(&fds[0], (
sizeof(fds) /
sizeof(fds[0])), 500);
104 if (fds[0].revents & POLLIN) {
108 }
else if (fds[1].revents & POLLIN) {
112 }
else if (fds[2].revents & POLLIN) {
118 PX4_ERR(
"poll error %d, %d", pret, errno);
122 num_missed += t.
val - current_value - 1;
123 current_value = t.
val;
126 latency_integral += elt;
129 if (elt > timing_max) {
133 if (elt < timing_min) {
144 sprintf(fname, PX4_STORAGEDIR
"/uorb_timings%u.txt", timingsgroup);
145 FILE *
f = fopen(fname,
"w");
148 PX4_ERR(
"Error opening file!");
153 for (
unsigned i = 0; i < maxruns; i++) {
154 fprintf(f,
"%u\n", timings[i]);
162 float mean = latency_integral / maxruns;
164 for (
unsigned i = 0; i < maxruns; i++) {
165 float diff = (float)timings[i] - mean;
166 std_dev += diff * diff;
171 PX4_INFO(
"mean: %8.4f us", static_cast<double>(mean));
172 PX4_INFO(
"std dev: %8.4f us", static_cast<double>(sqrtf(std_dev / (maxruns - 1))));
173 PX4_INFO(
"min: %3i us", timing_min);
174 PX4_INFO(
"max: %3i us", timing_max);
175 PX4_INFO(
"missed topic updates: %i", num_missed);
179 if (static_cast<float>(latency_integral / maxruns) > 100.0
f) {
235 for (
int i = 0; i < 4; ++i) {
239 return test_fail(
"orb_unadvertise failed (%i)", ret);
244 int instance_test[4];
247 for (
int i = 0; i < 4; ++i) {
250 if (instance_test[i] != i) {
251 return test_fail(
"got wrong instance (should be %i, is %i)", i, instance_test[i]);
255 for (
int i = 0; i < 4; ++i) {
280 if (ptopic ==
nullptr) {
281 return test_fail(
"advertise failed: %d", errno);
288 return test_fail(
"subscribe failed: %d", errno);
295 return test_fail(
"copy(1) failed: %d", errno);
302 if (PX4_OK !=
orb_check(sfd, &updated)) {
307 return test_fail(
"spurious updated flag");
317 if (PX4_OK !=
orb_check(sfd, &updated)) {
322 return test_fail(
"missing updated flag");
326 return test_fail(
"copy(2) failed: %d", errno);
338 return test_fail(
"orb_unadvertise failed: %i", ret);
341 return test_note(
"PASS single-topic test");
359 if (instance0 != 0) {
360 return test_fail(
"mult. id0: %d", instance0);
363 if (instance1 != 1) {
364 return test_fail(
"mult. id1: %d", instance1);
385 return test_fail(
"sub #0 copy failed: %d", errno);
389 return test_fail(
"sub #0 val. mismatch: %d", u.val);
395 return test_fail(
"sub #1 copy failed: %d", errno);
399 return test_fail(
"sub #1 val. mismatch: %d", u.val);
421 if (PX4_OK != latency_test<struct orb_test>(
ORB_ID(
orb_test),
false)) {
428 return test_note(
"PASS multi-topic test");
441 int data_next_idx = 0;
442 const int num_instances = 3;
446 for (
int i = 0; i < num_instances; ++i) {
454 PX4_ERR(
"Got wrong instance! should be: %i, but is %i", i, idx);
459 px4_usleep(100 * 1000);
461 int message_counter = 0, num_messages = 50 * num_instances;
463 while (message_counter++ < num_messages) {
468 data_topic.
val = data_next_idx;
473 data_next_idx = (data_next_idx + 1) % num_instances;
475 if (data_next_idx == 0) {
476 px4_usleep(50 * 1000);
480 px4_usleep(100 * 1000);
483 for (
int i = 0; i < num_instances; ++i) {
493 test_note(
"Testing multi-topic 2 test (queue simulation)");
497 const int num_instances = 3;
498 int orb_data_fd[num_instances];
499 int orb_data_next = 0;
501 for (
int i = 0; i < num_instances; ++i) {
506 char *
const args[1] = {
nullptr };
507 int pubsub_task = px4_task_spawn_cmd(
"uorb_test_multi",
509 SCHED_PRIORITY_MAX - 5,
514 if (pubsub_task < 0) {
515 return test_fail(
"failed launching task");
522 bool updated =
false;
523 int orb_data_cur_fd = orb_data_fd[orb_data_next];
537 if (last_time >= msg.
time && last_time != 0) {
538 return test_fail(
"Timestamp not increasing! (%" PRIu64
" >= %" PRIu64
")", last_time, msg.
time);
541 last_time = msg.
time;
544 orb_data_next = (orb_data_next + 1) % num_instances;
548 for (
int i = 0; i < num_instances; ++i) {
552 return test_note(
"PASS multi-topic 2 test (queue simulation)");
557 test_note(
"try multi-topic support subscribing before publishing");
565 return test_fail(
"sub. id2: ret: %d", sfd2);
582 if (instance2 != 2) {
583 return test_fail(
"mult. id2: %d", instance2);
586 if (instance3 != 3) {
587 return test_fail(
"mult. id3: %d", instance3);
606 return test_fail(
"sub #2 copy failed: %d", errno);
610 return test_fail(
"sub #3 val. mismatch: %d", u.val);
616 return test_fail(
"sub #3 copy failed: %d", errno);
620 return test_fail(
"sub #3 val. mismatch: %d", u.val);
623 return test_note(
"PASS multi-topic reversed");
638 return test_fail(
"subscribe failed: %d", errno);
642 const int queue_size = 11;
646 if (ptopic ==
nullptr) {
647 return test_fail(
"advertise failed: %d", errno);
657 return test_fail(
"copy(1) failed: %d", errno);
667 return test_fail(
"spurious updated flag");
670 #define CHECK_UPDATED(element) \ 671 orb_check(sfd, &updated); \ 673 return test_fail("update flag not set, element %i", element); \ 675 #define CHECK_NOT_UPDATED(element) \ 676 orb_check(sfd, &updated); \ 678 return test_fail("update flag set, element %i", element); \ 680 #define CHECK_COPY(i_got, i_correct) \ 681 orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ 682 if (i_got != i_correct) { \ 683 return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ 688 test_note(
" Testing to write some elements...");
690 for (
int i = 0; i < queue_size - 2; ++i) {
695 for (
int i = 0; i < queue_size - 2; ++i) {
705 for (
int i = 0; i < queue_size + overflow_by; ++i) {
710 for (
int i = 0; i < queue_size; ++i) {
719 for (
int i = 0; i < queue_size; ++i) {
731 #undef CHECK_NOT_UPDATED 749 const int queue_size = 50;
754 return test_fail(
"advertise failed: %d", errno);
757 int message_counter = 0, num_messages = 20 * queue_size;
760 while (message_counter < num_messages) {
763 int burst_counter = 0;
765 while (burst_counter++ < queue_size / 2 + 7) {
770 message_counter += burst_counter;
771 px4_usleep(20 * 1000);
775 px4_usleep(100 * 1000);
784 test_note(
"Testing orb queuing (poll & notify)");
790 return test_fail(
"subscribe failed: %d", errno);
795 char *
const args[1] = {
nullptr };
796 int pubsub_task = px4_task_spawn_cmd(
"uorb_test_queue",
798 SCHED_PRIORITY_MIN + 5,
803 if (pubsub_task < 0) {
804 return test_fail(
"failed launching task");
807 int next_expected_val = 0;
808 px4_pollfd_struct_t fds[1];
810 fds[0].events = POLLIN;
814 int poll_ret =
px4_poll(fds, 1, 500);
823 }
else if (poll_ret < 0) {
824 return test_fail(
"poll error (%d, %d)", poll_ret, errno);
827 if (fds[0].revents & POLLIN) {
830 if (next_expected_val != t.
val) {
831 return test_fail(
"copy mismatch: %d expected %d", t.
val, next_expected_val);
839 return test_fail(
"number of sent and received messages mismatch (sent: %i, received: %i)",
843 return test_note(
"PASS orb queuing (poll & notify), got %i messages", next_expected_val);
851 fprintf(stderr,
"uORB FAIL: ");
853 vfprintf(stderr, fmt, ap);
855 fprintf(stderr,
"\n");
865 fprintf(stderr,
"uORB note: ");
867 vfprintf(stderr, fmt, ap);
869 fprintf(stderr,
"\n");
static int pubsubtest_threadEntry(int argc, char *argv[])
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
int test_queue_poll_notify()
int test_multi_reversed()
volatile bool _thread_should_exit
int orb_priority(int handle, int32_t *priority)
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
#define CHECK_NOT_UPDATED(element)
int pub_test_multi2_main()
#define CHECK_COPY(i_got, i_correct)
orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
int orb_subscribe(const struct orb_metadata *meta)
volatile int _num_messages_sent
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
int orb_unsubscribe(int handle)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
#define CHECK_UPDATED(element)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
int test_note(const char *fmt,...)
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;")
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
int test_fail(const char *fmt,...)
static uORBTest::UnitTest & instance()
int orb_check(int handle, bool *updated)
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
int pub_test_queue_main()
int orb_unadvertise(orb_advert_t handle)
static int pub_test_queue_entry(int argc, char *argv[])
orb_advert_t _pfd[4]
used for test_multi and test_multi_reversed
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int pub_test_multi2_entry(int argc, char *argv[])
__EXPORT hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.