PX4 Firmware
PX4 Autopilot Software http://px4.io
test_microbench_uorb.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2018-2019 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 /**
35  * @file test_microbench_uorb.cpp
36  * Tests for microbench uORB functionality.
37  */
38 
39 #include <unit_test.h>
40 
41 #include <time.h>
42 #include <stdlib.h>
43 #include <unistd.h>
44 
45 #include <drivers/drv_hrt.h>
46 #include <perf/perf_counter.h>
47 #include <px4_platform_common/px4_config.h>
48 #include <px4_platform_common/micro_hal.h>
49 
50 #include <uORB/Subscription.hpp>
55 
56 namespace MicroBenchORB
57 {
58 
59 #ifdef __PX4_NUTTX
60 #include <nuttx/irq.h>
61 static irqstate_t flags;
62 #endif
63 
64 void lock()
65 {
66 #ifdef __PX4_NUTTX
67  flags = px4_enter_critical_section();
68 #endif
69 }
70 
71 void unlock()
72 {
73 #ifdef __PX4_NUTTX
74  px4_leave_critical_section(flags);
75 #endif
76 }
77 
78 #define PERF(name, op, count) do { \
79  px4_usleep(1000); \
80  reset(); \
81  perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
82  for (int i = 0; i < count; i++) { \
83  lock(); \
84  perf_begin(p); \
85  op; \
86  perf_end(p); \
87  unlock(); \
88  reset(); \
89  } \
90  perf_print_counter(p); \
91  perf_free(p); \
92  } while (0)
93 
94 class MicroBenchORB : public UnitTest
95 {
96 public:
97  virtual bool run_tests();
98 
99 private:
100 
101  bool time_px4_uorb();
102  bool time_px4_uorb_direct();
103 
104  void reset();
105 
109 };
110 
111 bool MicroBenchORB::run_tests()
112 {
113  ut_run_test(time_px4_uorb);
114  ut_run_test(time_px4_uorb_direct);
115 
116  return (_tests_failed == 0);
117 }
118 
119 template<typename T>
120 T random(T min, T max)
121 {
122  const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
123  return min + scale * (max - min); /* [min, max] */
124 }
125 
127 {
128  srand(time(nullptr));
129 
130  // initialize with random data
131  status.timestamp = rand();
132  status.mission_failure = rand();
133 
134  lpos.timestamp = rand();
135  lpos.dist_bottom_valid = rand();
136 
137  gyro.timestamp = rand();
138 }
139 
141 
142 bool MicroBenchORB::time_px4_uorb()
143 {
144  int fd_status = orb_subscribe(ORB_ID(vehicle_status));
145  int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
146  int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
147 
148  int ret = 0;
149  bool updated = false;
150  uint64_t time = 0;
151 
152  PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000);
153  PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
154  PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);
155 
156  printf("\n");
157 
158  PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
159  PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
160  PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);
161 
162  printf("\n");
163 
164  PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
165  PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
166  PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &gyro), 1000);
167 
168  printf("\n");
169 
170  PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
171  PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
172  PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100);
173  PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100);
174  PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100);
175 
176  orb_unsubscribe(fd_status);
177  orb_unsubscribe(fd_lpos);
178  orb_unsubscribe(fd_gyro);
179 
180  return true;
181 }
182 
183 bool MicroBenchORB::time_px4_uorb_direct()
184 {
185  bool ret = false;
186  bool updated = false;
187  uint64_t time = 0;
188 
189  uORB::Subscription vstatus{ORB_ID(vehicle_status)};
190  PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 1000);
191  PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 1000);
192 
193  printf("\n");
194 
195  uORB::Subscription local_pos{ORB_ID(vehicle_local_position)};
196  PERF("uORB::Subscription orb_check vehicle_local_position", ret = local_pos.updated(), 1000);
197  PERF("uORB::Subscription orb_copy vehicle_local_position", ret = local_pos.copy(&lpos), 1000);
198 
199  printf("\n");
200 
201  uORB::Subscription sens_gyro{ORB_ID(sensor_gyro)};
202  PERF("uORB::Subscription orb_check sensor_gyro", ret = sens_gyro.updated(), 1000);
203  PERF("uORB::Subscription orb_copy sensor_gyro", ret = sens_gyro.copy(&gyro), 1000);
204 
205  return true;
206 }
207 
208 } // namespace MicroBenchORB
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
static struct vehicle_status_s status
Definition: Commander.cpp:138
int reset(enum LPS22HB_BUS busid)
Reset the driver.
Base class to be used for unit tests.
Definition: unit_test.h:54
#define PERF(name, op, count)
int orb_exists(const struct orb_metadata *meta, int instance)
Definition: uORB.cpp:105
High-resolution timer with callouts and timekeeping.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int orb_stat(int handle, uint64_t *time)
Definition: uORB.cpp:100
ut_declare_test_c(test_microbench_uorb, MicroBenchORB) bool MicroBenchORB
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
T random(T min, T max)
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int test_microbench_uorb(int argc, char *argv[])
#define ut_run_test(test)
Runs a single unit test.
Definition: unit_test.h:96
Performance measuring tools.