47 #include <px4_platform_common/px4_config.h> 48 #include <px4_platform_common/micro_hal.h> 56 #include <nuttx/irq.h> 57 static irqstate_t flags;
63 flags = px4_enter_critical_section();
70 px4_leave_critical_section(flags);
74 #define PERF(name, op, count) do { \ 77 perf_counter_t p = perf_alloc(PC_ELAPSED, name); \ 78 for (int i = 0; i < count; i++) { \ 86 perf_print_counter(p); \ 93 virtual bool run_tests();
97 bool time_px4_matrix();
109 bool MicroBenchMatrix::run_tests()
113 return (_tests_failed == 0);
119 const T scale = rand() / (T) RAND_MAX;
120 return min + scale * (max -
min);
125 srand(time(
nullptr));
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);
137 for (
size_t i = 0; i < 4; i++) {
138 B16_4(j, i) =
random(-10.0, 10.0);
146 bool MicroBenchMatrix::time_px4_matrix()
148 PERF(
"matrix Euler from Quaternion", e = q, 1000);
149 PERF(
"matrix Euler from Dcm", e = d, 1000);
151 PERF(
"matrix Quaternion from Euler", q = e, 1000);
152 PERF(
"matrix Quaternion from Dcm", q = d, 1000);
154 PERF(
"matrix Dcm from Euler", d = e, 1000);
155 PERF(
"matrix Dcm from Quaternion", d = q, 1000);
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);
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.
High-resolution timer with callouts and timekeeping.
#define PERF(name, op, count)
constexpr _Tp min(_Tp a, _Tp b)
matrix::Matrix< float, 6, 16 > B16
matrix::Matrix< float, 16, 6 > A16
constexpr _Tp max(_Tp a, _Tp b)
Quaternion< float > Quatf
matrix::Matrix< float, 6, 16 > B16_4
#define ut_run_test(test)
Runs a single unit test.
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.