PX4 Firmware
PX4 Autopilot Software http://px4.io
uORBTest_UnitTest.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include "uORBTest_UnitTest.hpp"
35 #include "../uORBCommon.hpp"
36 #include <px4_platform_common/px4_config.h>
37 #include <px4_platform_common/time.h>
38 #include <stdio.h>
39 #include <errno.h>
40 #include <poll.h>
41 #include <math.h>
42 #include <lib/cdev/CDev.hpp>
43 
44 ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;");
45 ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;");
46 
48  "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
49 ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
50  "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
51 ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium),
52  "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
53 ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium),
54  "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
55 
57  "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
58 
60 {
61  static uORBTest::UnitTest t;
62  return t;
63 }
64 
66 {
67  /* poll on test topic and output latency */
68  float latency_integral = 0.0f;
69 
70  /* wakeup source(s) */
71  px4_pollfd_struct_t fds[3];
72 
73  int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
74  int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
75  int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);
76 
77  struct orb_test_large t;
78 
79  /* clear all ready flags */
80  orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
81  orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
82  orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
83 
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;
90 
91  const unsigned maxruns = 1000;
92  unsigned timingsgroup = 0;
93  int current_value = t.val;
94  int num_missed = 0;
95 
96  // timings has to be on the heap to keep frame size below 2048 bytes
97  unsigned *timings = new unsigned[maxruns];
98  unsigned timing_min = 9999999, timing_max = 0;
99 
100  for (unsigned i = 0; i < maxruns; i++) {
101  /* wait for up to 500ms for data */
102  int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
103 
104  if (fds[0].revents & POLLIN) {
105  orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
106  timingsgroup = 0;
107 
108  } else if (fds[1].revents & POLLIN) {
109  orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
110  timingsgroup = 1;
111 
112  } else if (fds[2].revents & POLLIN) {
113  orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
114  timingsgroup = 2;
115  }
116 
117  if (pret < 0) {
118  PX4_ERR("poll error %d, %d", pret, errno);
119  continue;
120  }
121 
122  num_missed += t.val - current_value - 1;
123  current_value = t.val;
124 
125  unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.time);
126  latency_integral += elt;
127  timings[i] = elt;
128 
129  if (elt > timing_max) {
130  timing_max = elt;
131  }
132 
133  if (elt < timing_min) {
134  timing_min = elt;
135  }
136  }
137 
138  orb_unsubscribe(test_multi_sub);
139  orb_unsubscribe(test_multi_sub_medium);
140  orb_unsubscribe(test_multi_sub_large);
141 
142  if (pubsubtest_print) {
143  char fname[32];
144  sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
145  FILE *f = fopen(fname, "w");
146 
147  if (f == nullptr) {
148  PX4_ERR("Error opening file!");
149  delete[] timings;
150  return PX4_ERROR;
151  }
152 
153  for (unsigned i = 0; i < maxruns; i++) {
154  fprintf(f, "%u\n", timings[i]);
155  }
156 
157  fclose(f);
158  }
159 
160 
161  float std_dev = 0.f;
162  float mean = latency_integral / maxruns;
163 
164  for (unsigned i = 0; i < maxruns; i++) {
165  float diff = (float)timings[i] - mean;
166  std_dev += diff * diff;
167  }
168 
169  delete[] timings;
170 
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);
176 
177  pubsubtest_passed = true;
178 
179  if (static_cast<float>(latency_integral / maxruns) > 100.0f) {
180  pubsubtest_res = PX4_ERROR;
181 
182  } else {
183  pubsubtest_res = PX4_OK;
184  }
185 
186  return pubsubtest_res;
187 }
188 
190 {
191  int ret = test_single();
192 
193  if (ret != OK) {
194  return ret;
195  }
196 
197  ret = test_multi();
198 
199  if (ret != OK) {
200  return ret;
201  }
202 
203  ret = test_multi_reversed();
204 
205  if (ret != OK) {
206  return ret;
207  }
208 
209  ret = test_unadvertise();
210 
211  if (ret != OK) {
212  return ret;
213  }
214 
215  ret = test_multi2();
216 
217  if (ret != OK) {
218  return ret;
219  }
220 
221  ret = test_queue();
222 
223  if (ret != OK) {
224  return ret;
225  }
226 
227  return test_queue_poll_notify();
228 }
229 
231 {
232  test_note("Testing unadvertise");
233 
234  //we still have the advertisements from the previous test_multi calls.
235  for (int i = 0; i < 4; ++i) {
236  int ret = orb_unadvertise(_pfd[i]);
237 
238  if (ret != PX4_OK) {
239  return test_fail("orb_unadvertise failed (%i)", ret);
240  }
241  }
242 
243  //try to advertise and see whether we get the right instance
244  int instance_test[4];
245  struct orb_test t;
246 
247  for (int i = 0; i < 4; ++i) {
248  _pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);
249 
250  if (instance_test[i] != i) {
251  return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
252  }
253  }
254 
255  for (int i = 0; i < 4; ++i) {
256  orb_unadvertise(_pfd[i]);
257  }
258 
259  return test_note("PASS unadvertise");
260 }
261 
262 
264 {
265  return OK;
266 }
267 
269 {
270  test_note("try single-topic support");
271 
272  struct orb_test t, u;
273  int sfd;
274  orb_advert_t ptopic;
275  bool updated;
276 
277  t.val = 0;
278  ptopic = orb_advertise(ORB_ID(orb_test), &t);
279 
280  if (ptopic == nullptr) {
281  return test_fail("advertise failed: %d", errno);
282  }
283 
284  test_note("publish handle %p", ptopic);
285  sfd = orb_subscribe(ORB_ID(orb_test));
286 
287  if (sfd < 0) {
288  return test_fail("subscribe failed: %d", errno);
289  }
290 
291  test_note("subscribe fd %d", sfd);
292  u.val = 1;
293 
294  if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
295  return test_fail("copy(1) failed: %d", errno);
296  }
297 
298  if (u.val != t.val) {
299  return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
300  }
301 
302  if (PX4_OK != orb_check(sfd, &updated)) {
303  return test_fail("check(1) failed");
304  }
305 
306  if (updated) {
307  return test_fail("spurious updated flag");
308  }
309 
310  t.val = 2;
311  test_note("try publish");
312 
313  if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) {
314  return test_fail("publish failed");
315  }
316 
317  if (PX4_OK != orb_check(sfd, &updated)) {
318  return test_fail("check(2) failed");
319  }
320 
321  if (!updated) {
322  return test_fail("missing updated flag");
323  }
324 
325  if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
326  return test_fail("copy(2) failed: %d", errno);
327  }
328 
329  if (u.val != t.val) {
330  return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
331  }
332 
333  orb_unsubscribe(sfd);
334 
335  int ret = orb_unadvertise(ptopic);
336 
337  if (ret != PX4_OK) {
338  return test_fail("orb_unadvertise failed: %i", ret);
339  }
340 
341  return test_note("PASS single-topic test");
342 }
343 
345 {
346  /* this routine tests the multi-topic support */
347  test_note("try multi-topic support");
348 
349  struct orb_test t {}, u {};
350  t.val = 0;
351  int instance0;
352  _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
353 
354  test_note("advertised");
355 
356  int instance1;
357  _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
358 
359  if (instance0 != 0) {
360  return test_fail("mult. id0: %d", instance0);
361  }
362 
363  if (instance1 != 1) {
364  return test_fail("mult. id1: %d", instance1);
365  }
366 
367  t.val = 103;
368 
369  if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) {
370  return test_fail("mult. pub0 fail");
371  }
372 
373  test_note("published");
374 
375  t.val = 203;
376 
377  if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) {
378  return test_fail("mult. pub1 fail");
379  }
380 
381  /* subscribe to both topics and ensure valid data is received */
382  int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
383 
384  if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) {
385  return test_fail("sub #0 copy failed: %d", errno);
386  }
387 
388  if (u.val != 103) {
389  return test_fail("sub #0 val. mismatch: %d", u.val);
390  }
391 
392  int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
393 
394  if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) {
395  return test_fail("sub #1 copy failed: %d", errno);
396  }
397 
398  if (u.val != 203) {
399  return test_fail("sub #1 val. mismatch: %d", u.val);
400  }
401 
402  /* test priorities */
403  int prio;
404 
405  if (PX4_OK != orb_priority(sfd0, &prio)) {
406  return test_fail("prio #0");
407  }
408 
409  if (prio != ORB_PRIO_MAX) {
410  return test_fail("prio: %d", prio);
411  }
412 
413  if (PX4_OK != orb_priority(sfd1, &prio)) {
414  return test_fail("prio #1");
415  }
416 
417  if (prio != ORB_PRIO_MIN) {
418  return test_fail("prio: %d", prio);
419  }
420 
421  if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) {
422  return test_fail("latency test failed");
423  }
424 
425  orb_unsubscribe(sfd0);
426  orb_unsubscribe(sfd1);
427 
428  return test_note("PASS multi-topic test");
429 }
430 
431 
432 
433 int uORBTest::UnitTest::pub_test_multi2_entry(int argc, char *argv[])
434 {
436  return t.pub_test_multi2_main();
437 }
438 
440 {
441  int data_next_idx = 0;
442  const int num_instances = 3;
443  orb_advert_t orb_pub[num_instances];
444  struct orb_test_medium data_topic;
445 
446  for (int i = 0; i < num_instances; ++i) {
447  orb_advert_t &pub = orb_pub[i];
448  int idx = i;
449 // PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
450  pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);
451 
452  if (idx != i) {
453  _thread_should_exit = true;
454  PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx);
455  return -1;
456  }
457  }
458 
459  px4_usleep(100 * 1000);
460 
461  int message_counter = 0, num_messages = 50 * num_instances;
462 
463  while (message_counter++ < num_messages) {
464  px4_usleep(2); //make sure the timestamps are different
465  orb_advert_t &pub = orb_pub[data_next_idx];
466 
467  data_topic.time = hrt_absolute_time();
468  data_topic.val = data_next_idx;
469 
470  orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
471 // PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time);
472 
473  data_next_idx = (data_next_idx + 1) % num_instances;
474 
475  if (data_next_idx == 0) {
476  px4_usleep(50 * 1000);
477  }
478  }
479 
480  px4_usleep(100 * 1000);
481  _thread_should_exit = true;
482 
483  for (int i = 0; i < num_instances; ++i) {
484  orb_unadvertise(orb_pub[i]);
485  }
486 
487  return 0;
488 }
489 
491 {
492 
493  test_note("Testing multi-topic 2 test (queue simulation)");
494  //test: first subscribe, then advertise
495 
496  _thread_should_exit = false;
497  const int num_instances = 3;
498  int orb_data_fd[num_instances];
499  int orb_data_next = 0;
500 
501  for (int i = 0; i < num_instances; ++i) {
502 // PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
503  orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
504  }
505 
506  char *const args[1] = { nullptr };
507  int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
508  SCHED_DEFAULT,
509  SCHED_PRIORITY_MAX - 5,
510  2000,
512  args);
513 
514  if (pubsub_task < 0) {
515  return test_fail("failed launching task");
516  }
517 
518  hrt_abstime last_time = 0;
519 
520  while (!_thread_should_exit) {
521 
522  bool updated = false;
523  int orb_data_cur_fd = orb_data_fd[orb_data_next];
524  orb_check(orb_data_cur_fd, &updated);
525 
526  if (updated) {
527  struct orb_test_medium msg;
528  orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
529 
530 // Relax timing requirement for Darwin CI system
531 #ifdef __PX4_DARWIN
532  px4_usleep(10000);
533 #else
534  px4_usleep(1000);
535 #endif
536 
537  if (last_time >= msg.time && last_time != 0) {
538  return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
539  }
540 
541  last_time = msg.time;
542 
543 // PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
544  orb_data_next = (orb_data_next + 1) % num_instances;
545  }
546  }
547 
548  for (int i = 0; i < num_instances; ++i) {
549  orb_unsubscribe(orb_data_fd[i]);
550  }
551 
552  return test_note("PASS multi-topic 2 test (queue simulation)");
553 }
554 
556 {
557  test_note("try multi-topic support subscribing before publishing");
558 
559  /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */
560 
561  /* Subscribe first and advertise afterwards. */
562  int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2);
563 
564  if (sfd2 < 0) {
565  return test_fail("sub. id2: ret: %d", sfd2);
566  }
567 
568  struct orb_test t {}, u {};
569 
570  t.val = 0;
571 
572  int instance2;
573 
574  _pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX);
575 
576  int instance3;
577 
578  _pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN);
579 
580  test_note("advertised");
581 
582  if (instance2 != 2) {
583  return test_fail("mult. id2: %d", instance2);
584  }
585 
586  if (instance3 != 3) {
587  return test_fail("mult. id3: %d", instance3);
588  }
589 
590  t.val = 204;
591 
592  if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) {
593  return test_fail("mult. pub0 fail");
594  }
595 
596 
597  t.val = 304;
598 
599  if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) {
600  return test_fail("mult. pub1 fail");
601  }
602 
603  test_note("published");
604 
605  if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) {
606  return test_fail("sub #2 copy failed: %d", errno);
607  }
608 
609  if (u.val != 204) {
610  return test_fail("sub #3 val. mismatch: %d", u.val);
611  }
612 
613  int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3);
614 
615  if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) {
616  return test_fail("sub #3 copy failed: %d", errno);
617  }
618 
619  if (u.val != 304) {
620  return test_fail("sub #3 val. mismatch: %d", u.val);
621  }
622 
623  return test_note("PASS multi-topic reversed");
624 }
625 
627 {
628  test_note("Testing orb queuing");
629 
630  struct orb_test_medium t, u;
631  int sfd;
632  orb_advert_t ptopic;
633  bool updated;
634 
635  sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
636 
637  if (sfd < 0) {
638  return test_fail("subscribe failed: %d", errno);
639  }
640 
641 
642  const int queue_size = 11;
643  t.val = 0;
644  ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
645 
646  if (ptopic == nullptr) {
647  return test_fail("advertise failed: %d", errno);
648  }
649 
650  orb_check(sfd, &updated);
651 
652  if (!updated) {
653  return test_fail("update flag not set");
654  }
655 
656  if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) {
657  return test_fail("copy(1) failed: %d", errno);
658  }
659 
660  if (u.val != t.val) {
661  return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
662  }
663 
664  orb_check(sfd, &updated);
665 
666  if (updated) {
667  return test_fail("spurious updated flag");
668  }
669 
670 #define CHECK_UPDATED(element) \
671  orb_check(sfd, &updated); \
672  if (!updated) { \
673  return test_fail("update flag not set, element %i", element); \
674  }
675 #define CHECK_NOT_UPDATED(element) \
676  orb_check(sfd, &updated); \
677  if (updated) { \
678  return test_fail("update flag set, element %i", element); \
679  }
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); \
684  }
685 
686  //no messages in the queue anymore
687 
688  test_note(" Testing to write some elements...");
689 
690  for (int i = 0; i < queue_size - 2; ++i) {
691  t.val = i;
692  orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
693  }
694 
695  for (int i = 0; i < queue_size - 2; ++i) {
696  CHECK_UPDATED(i);
697  CHECK_COPY(u.val, i);
698  }
699 
700  CHECK_NOT_UPDATED(queue_size);
701 
702  test_note(" Testing overflow...");
703  int overflow_by = 3;
704 
705  for (int i = 0; i < queue_size + overflow_by; ++i) {
706  t.val = i;
707  orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
708  }
709 
710  for (int i = 0; i < queue_size; ++i) {
711  CHECK_UPDATED(i);
712  CHECK_COPY(u.val, i + overflow_by);
713  }
714 
715  CHECK_NOT_UPDATED(queue_size);
716 
717  test_note(" Testing underflow...");
718 
719  for (int i = 0; i < queue_size; ++i) {
721  CHECK_COPY(u.val, queue_size + overflow_by - 1);
722  }
723 
724  t.val = 943;
725  orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
726  CHECK_UPDATED(-1);
727  CHECK_COPY(u.val, t.val);
728 
729 #undef CHECK_COPY
730 #undef CHECK_UPDATED
731 #undef CHECK_NOT_UPDATED
732 
733  orb_unadvertise(ptopic);
734 
735  return test_note("PASS orb queuing");
736 }
737 
738 
739 int uORBTest::UnitTest::pub_test_queue_entry(int argc, char *argv[])
740 {
742  return t.pub_test_queue_main();
743 }
744 
746 {
747  struct orb_test_medium t;
748  orb_advert_t ptopic;
749  const int queue_size = 50;
750  t.val = 0;
751 
752  if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
753  _thread_should_exit = true;
754  return test_fail("advertise failed: %d", errno);
755  }
756 
757  int message_counter = 0, num_messages = 20 * queue_size;
758  ++t.val;
759 
760  while (message_counter < num_messages) {
761 
762  //simulate burst
763  int burst_counter = 0;
764 
765  while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned
766  orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t);
767  ++t.val;
768  }
769 
770  message_counter += burst_counter;
771  px4_usleep(20 * 1000); //give subscriber a chance to catch up
772  }
773 
775  px4_usleep(100 * 1000);
776  _thread_should_exit = true;
777  orb_unadvertise(ptopic);
778 
779  return 0;
780 }
781 
783 {
784  test_note("Testing orb queuing (poll & notify)");
785 
786  struct orb_test_medium t;
787  int sfd;
788 
789  if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) {
790  return test_fail("subscribe failed: %d", errno);
791  }
792 
793  _thread_should_exit = false;
794 
795  char *const args[1] = { nullptr };
796  int pubsub_task = px4_task_spawn_cmd("uorb_test_queue",
797  SCHED_DEFAULT,
798  SCHED_PRIORITY_MIN + 5,
799  1500,
801  args);
802 
803  if (pubsub_task < 0) {
804  return test_fail("failed launching task");
805  }
806 
807  int next_expected_val = 0;
808  px4_pollfd_struct_t fds[1];
809  fds[0].fd = sfd;
810  fds[0].events = POLLIN;
811 
812  while (!_thread_should_exit) {
813 
814  int poll_ret = px4_poll(fds, 1, 500);
815 
816  if (poll_ret == 0) {
817  if (_thread_should_exit) {
818  break;
819  }
820 
821  return test_fail("poll timeout");
822 
823  } else if (poll_ret < 0) {
824  return test_fail("poll error (%d, %d)", poll_ret, errno);
825  }
826 
827  if (fds[0].revents & POLLIN) {
828  orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t);
829 
830  if (next_expected_val != t.val) {
831  return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val);
832  }
833 
834  ++next_expected_val;
835  }
836  }
837 
838  if (_num_messages_sent != next_expected_val) {
839  return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)",
840  _num_messages_sent, next_expected_val);
841  }
842 
843  return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val);
844 }
845 
846 
847 int uORBTest::UnitTest::test_fail(const char *fmt, ...)
848 {
849  va_list ap;
850 
851  fprintf(stderr, "uORB FAIL: ");
852  va_start(ap, fmt);
853  vfprintf(stderr, fmt, ap);
854  va_end(ap);
855  fprintf(stderr, "\n");
856  fflush(stderr);
857 
858  return PX4_ERROR;
859 }
860 
861 int uORBTest::UnitTest::test_note(const char *fmt, ...)
862 {
863  va_list ap;
864 
865  fprintf(stderr, "uORB note: ");
866  va_start(ap, fmt);
867  vfprintf(stderr, fmt, ap);
868  va_end(ap);
869  fprintf(stderr, "\n");
870  fflush(stderr);
871  return OK;
872 }
873 
875 {
877  return t.pubsublatency_main();
878 }
static int pubsubtest_threadEntry(int argc, char *argv[])
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
volatile bool _thread_should_exit
int orb_priority(int handle, int32_t *priority)
Definition: uORB.cpp:121
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
#define CHECK_NOT_UPDATED(element)
#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)
Definition: uORB.cpp:48
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
volatile int _num_messages_sent
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define CHECK_UPDATED(element)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
int test_note(const char *fmt,...)
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
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)
Definition: uORB.cpp:70
int test_fail(const char *fmt,...)
static uORBTest::UnitTest & instance()
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition: uORB.cpp:80
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
#define OK
Definition: uavcan_main.cpp:71
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)
Definition: uORB.cpp:53
__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.