PX4 Firmware
PX4 Autopilot Software http://px4.io
test_microbench_matrix.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_matrix.cpp
36  * Tests for the microbench matrix math library.
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 <matrix/math.hpp>
51 
53 {
54 
55 #ifdef __PX4_NUTTX
56 #include <nuttx/irq.h>
57 static irqstate_t flags;
58 #endif
59 
60 void lock()
61 {
62 #ifdef __PX4_NUTTX
63  flags = px4_enter_critical_section();
64 #endif
65 }
66 
67 void unlock()
68 {
69 #ifdef __PX4_NUTTX
70  px4_leave_critical_section(flags);
71 #endif
72 }
73 
74 #define PERF(name, op, count) do { \
75  px4_usleep(1000); \
76  reset(); \
77  perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
78  for (int i = 0; i < count; i++) { \
79  lock(); \
80  perf_begin(p); \
81  op; \
82  perf_end(p); \
83  unlock(); \
84  reset(); \
85  } \
86  perf_print_counter(p); \
87  perf_free(p); \
88  } while (0)
89 
90 class MicroBenchMatrix : public UnitTest
91 {
92 public:
93  virtual bool run_tests();
94 
95 private:
96 
97  bool time_px4_matrix();
98 
99  void reset();
100 
107 };
108 
109 bool MicroBenchMatrix::run_tests()
110 {
111  ut_run_test(time_px4_matrix);
112 
113  return (_tests_failed == 0);
114 }
115 
116 template<typename T>
117 T random(T min, T max)
118 {
119  const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
120  return min + scale * (max - min); /* [min, max] */
121 }
122 
124 {
125  srand(time(nullptr));
126 
127  // initialize with random data
128  q = matrix::Quatf(rand(), rand(), rand(), rand());
129  e = matrix::Eulerf(random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI));
130  d = q;
131 
132  for (size_t j = 0; j < 6; j++) {
133  for (size_t i = 0; i < 16; i++) {
134  B16(j, i) = random(-10.0, 10.0);
135  }
136 
137  for (size_t i = 0; i < 4; i++) {
138  B16_4(j, i) = random(-10.0, 10.0);
139  }
140  }
141 
142 }
143 
145 
146 bool MicroBenchMatrix::time_px4_matrix()
147 {
148  PERF("matrix Euler from Quaternion", e = q, 1000);
149  PERF("matrix Euler from Dcm", e = d, 1000);
150 
151  PERF("matrix Quaternion from Euler", q = e, 1000);
152  PERF("matrix Quaternion from Dcm", q = d, 1000);
153 
154  PERF("matrix Dcm from Euler", d = e, 1000);
155  PERF("matrix Dcm from Quaternion", d = q, 1000);
156 
157  PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 1000);
158  PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 1000);
159 
160  return true;
161 }
162 
163 } // namespace MicroBenchMatrix
ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix) bool MicroBenchMatrix
int reset(enum LPS22HB_BUS busid)
Reset the driver.
Base class to be used for unit tests.
Definition: unit_test.h:54
Quaternion class.
Definition: Dcm.hpp:24
High-resolution timer with callouts and timekeeping.
#define PERF(name, op, count)
Euler< float > Eulerf
Definition: Euler.hpp:156
Euler angles class.
Definition: AxisAngle.hpp:18
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
matrix::Matrix< float, 6, 16 > B16
matrix::Matrix< float, 16, 6 > A16
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
#define M_PI
Definition: gps_helper.cpp:38
matrix::Matrix< float, 6, 16 > B16_4
#define ut_run_test(test)
Runs a single unit test.
Definition: unit_test.h:96
int test_microbench_matrix(int argc, char *argv[])
Performance measuring tools.
Matrix< Type, N, M > geninv(const Matrix< Type, M, N > &G)
Geninv Fast pseudoinverse based on full rank cholesky factorisation.