Daniel Agar
7 years ago
committed by
Lorenz Meier
8 changed files with 525 additions and 117 deletions
@ -0,0 +1,144 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2018 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#include <unit_test.h> |
||||||
|
|
||||||
|
#include <time.h> |
||||||
|
#include <stdlib.h> |
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h> |
||||||
|
#include <perf/perf_counter.h> |
||||||
|
#include <px4_config.h> |
||||||
|
#include <px4_micro_hal.h> |
||||||
|
|
||||||
|
namespace MicroBenchHRT |
||||||
|
{ |
||||||
|
|
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
#include <nuttx/irq.h> |
||||||
|
static irqstate_t flags; |
||||||
|
#endif |
||||||
|
|
||||||
|
void lock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
flags = px4_enter_critical_section(); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void unlock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
px4_leave_critical_section(flags); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
#define PERF(name, op, count) do { \ |
||||||
|
usleep(1000); \
|
||||||
|
reset(); \
|
||||||
|
perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
|
||||||
|
for (int i = 0; i < count; i++) { \
|
||||||
|
lock(); \
|
||||||
|
perf_begin(p); \
|
||||||
|
op; \
|
||||||
|
perf_end(p); \
|
||||||
|
unlock(); \
|
||||||
|
reset(); \
|
||||||
|
} \
|
||||||
|
perf_print_counter(p); \
|
||||||
|
perf_free(p); \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
class MicroBenchHRT : public UnitTest |
||||||
|
{ |
||||||
|
public: |
||||||
|
virtual bool run_tests(); |
||||||
|
|
||||||
|
private: |
||||||
|
|
||||||
|
bool time_px4_hrt(); |
||||||
|
|
||||||
|
void reset(); |
||||||
|
|
||||||
|
void lock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
flags = px4_enter_critical_section(); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void unlock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
px4_leave_critical_section(flags); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
uint64_t u_64; |
||||||
|
uint64_t u_64_out; |
||||||
|
}; |
||||||
|
|
||||||
|
bool MicroBenchHRT::run_tests() |
||||||
|
{ |
||||||
|
ut_run_test(time_px4_hrt); |
||||||
|
|
||||||
|
return (_tests_failed == 0); |
||||||
|
} |
||||||
|
|
||||||
|
template<typename T> |
||||||
|
T random(T min, T max) |
||||||
|
{ |
||||||
|
const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */ |
||||||
|
return min + scale * (max - min); /* [min, max] */ |
||||||
|
} |
||||||
|
|
||||||
|
void MicroBenchHRT::reset() |
||||||
|
{ |
||||||
|
srand(time(nullptr)); |
||||||
|
|
||||||
|
// initialize with random data
|
||||||
|
u_64 = rand(); |
||||||
|
u_64_out = rand(); |
||||||
|
} |
||||||
|
|
||||||
|
ut_declare_test_c(test_microbench_hrt, MicroBenchHRT) |
||||||
|
|
||||||
|
bool MicroBenchHRT::time_px4_hrt() |
||||||
|
{ |
||||||
|
PERF("hrt_absolute_time()", u_64_out = hrt_absolute_time(), 1000); |
||||||
|
PERF("hrt_elapsed_time()", u_64_out = hrt_elapsed_time(&u_64), 1000); |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
} // namespace MicroBenchHRT
|
||||||
@ -0,0 +1,141 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2018 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#include <unit_test.h> |
||||||
|
|
||||||
|
#include <time.h> |
||||||
|
#include <stdlib.h> |
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h> |
||||||
|
#include <perf/perf_counter.h> |
||||||
|
#include <px4_config.h> |
||||||
|
#include <px4_micro_hal.h> |
||||||
|
|
||||||
|
#include <matrix/math.hpp> |
||||||
|
|
||||||
|
namespace MicroBenchMatrix |
||||||
|
{ |
||||||
|
|
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
#include <nuttx/irq.h> |
||||||
|
static irqstate_t flags; |
||||||
|
#endif |
||||||
|
|
||||||
|
void lock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
flags = px4_enter_critical_section(); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void unlock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
px4_leave_critical_section(flags); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
#define PERF(name, op, count) do { \ |
||||||
|
usleep(1000); \
|
||||||
|
reset(); \
|
||||||
|
perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
|
||||||
|
for (int i = 0; i < count; i++) { \
|
||||||
|
lock(); \
|
||||||
|
perf_begin(p); \
|
||||||
|
op; \
|
||||||
|
perf_end(p); \
|
||||||
|
unlock(); \
|
||||||
|
reset(); \
|
||||||
|
} \
|
||||||
|
perf_print_counter(p); \
|
||||||
|
perf_free(p); \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
class MicroBenchMatrix : public UnitTest |
||||||
|
{ |
||||||
|
public: |
||||||
|
virtual bool run_tests(); |
||||||
|
|
||||||
|
private: |
||||||
|
|
||||||
|
bool time_px4_matrix(); |
||||||
|
|
||||||
|
void reset(); |
||||||
|
|
||||||
|
matrix::Quatf q; |
||||||
|
matrix::Eulerf e; |
||||||
|
matrix::Dcmf d; |
||||||
|
|
||||||
|
}; |
||||||
|
|
||||||
|
bool MicroBenchMatrix::run_tests() |
||||||
|
{ |
||||||
|
ut_run_test(time_px4_matrix); |
||||||
|
|
||||||
|
return (_tests_failed == 0); |
||||||
|
} |
||||||
|
|
||||||
|
template<typename T> |
||||||
|
T random(T min, T max) |
||||||
|
{ |
||||||
|
const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */ |
||||||
|
return min + scale * (max - min); /* [min, max] */ |
||||||
|
} |
||||||
|
|
||||||
|
void MicroBenchMatrix::reset() |
||||||
|
{ |
||||||
|
srand(time(nullptr)); |
||||||
|
|
||||||
|
// initialize with random data
|
||||||
|
q = matrix::Quatf(rand(), rand(), rand(), rand()); |
||||||
|
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)); |
||||||
|
d = q; |
||||||
|
} |
||||||
|
|
||||||
|
ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix) |
||||||
|
|
||||||
|
bool MicroBenchMatrix::time_px4_matrix() |
||||||
|
{ |
||||||
|
PERF("matrix Euler from Quaternion", e = q, 1000); |
||||||
|
PERF("matrix Euler from Dcm", e = d, 1000); |
||||||
|
|
||||||
|
PERF("matrix Quaternion from Euler", q = e, 1000); |
||||||
|
PERF("matrix Quaternion from Dcm", q = d, 1000); |
||||||
|
|
||||||
|
PERF("matrix Dcm from Euler", d = e, 1000); |
||||||
|
PERF("matrix Dcm from Quaternion", d = q, 1000); |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
} // namespace MicroBenchMatrix
|
||||||
@ -0,0 +1,169 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2018 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#include <unit_test.h> |
||||||
|
|
||||||
|
#include <time.h> |
||||||
|
#include <stdlib.h> |
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h> |
||||||
|
#include <perf/perf_counter.h> |
||||||
|
#include <px4_config.h> |
||||||
|
#include <px4_micro_hal.h> |
||||||
|
|
||||||
|
#include <uORB/topics/sensor_accel.h> |
||||||
|
#include <uORB/topics/sensor_gyro.h> |
||||||
|
#include <uORB/topics/vehicle_local_position.h> |
||||||
|
#include <uORB/topics/vehicle_status.h> |
||||||
|
|
||||||
|
namespace MicroBenchORB |
||||||
|
{ |
||||||
|
|
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
#include <nuttx/irq.h> |
||||||
|
static irqstate_t flags; |
||||||
|
#endif |
||||||
|
|
||||||
|
void lock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
flags = px4_enter_critical_section(); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void unlock() |
||||||
|
{ |
||||||
|
#ifdef __PX4_NUTTX |
||||||
|
px4_leave_critical_section(flags); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
#define PERF(name, op, count) do { \ |
||||||
|
usleep(1000); \
|
||||||
|
reset(); \
|
||||||
|
perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
|
||||||
|
for (int i = 0; i < count; i++) { \
|
||||||
|
lock(); \
|
||||||
|
perf_begin(p); \
|
||||||
|
op; \
|
||||||
|
perf_end(p); \
|
||||||
|
unlock(); \
|
||||||
|
reset(); \
|
||||||
|
} \
|
||||||
|
perf_print_counter(p); \
|
||||||
|
perf_free(p); \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
class MicroBenchORB : public UnitTest |
||||||
|
{ |
||||||
|
public: |
||||||
|
virtual bool run_tests(); |
||||||
|
|
||||||
|
private: |
||||||
|
|
||||||
|
bool time_px4_uorb(); |
||||||
|
|
||||||
|
void reset(); |
||||||
|
|
||||||
|
vehicle_status_s status; |
||||||
|
vehicle_local_position_s lpos; |
||||||
|
sensor_gyro_s gyro; |
||||||
|
}; |
||||||
|
|
||||||
|
bool MicroBenchORB::run_tests() |
||||||
|
{ |
||||||
|
ut_run_test(time_px4_uorb); |
||||||
|
|
||||||
|
return (_tests_failed == 0); |
||||||
|
} |
||||||
|
|
||||||
|
template<typename T> |
||||||
|
T random(T min, T max) |
||||||
|
{ |
||||||
|
const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */ |
||||||
|
return min + scale * (max - min); /* [min, max] */ |
||||||
|
} |
||||||
|
|
||||||
|
void MicroBenchORB::reset() |
||||||
|
{ |
||||||
|
srand(time(nullptr)); |
||||||
|
|
||||||
|
// initialize with random data
|
||||||
|
status.timestamp = rand(); |
||||||
|
status.mission_failure = rand(); |
||||||
|
|
||||||
|
lpos.timestamp = rand(); |
||||||
|
lpos.dist_bottom_valid = rand(); |
||||||
|
|
||||||
|
gyro.timestamp = rand(); |
||||||
|
gyro.temperature_raw = rand(); |
||||||
|
} |
||||||
|
|
||||||
|
ut_declare_test_c(test_microbench_uorb, MicroBenchORB) |
||||||
|
|
||||||
|
bool MicroBenchORB::time_px4_uorb() |
||||||
|
{ |
||||||
|
int fd_status = orb_subscribe(ORB_ID(vehicle_status)); |
||||||
|
int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position)); |
||||||
|
int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro)); |
||||||
|
|
||||||
|
int ret = 0; |
||||||
|
bool updated = false; |
||||||
|
uint64_t time = 0; |
||||||
|
|
||||||
|
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000); |
||||||
|
PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000); |
||||||
|
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000); |
||||||
|
|
||||||
|
PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000); |
||||||
|
PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000); |
||||||
|
PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000); |
||||||
|
|
||||||
|
PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000); |
||||||
|
PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000); |
||||||
|
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &lpos), 1000); |
||||||
|
|
||||||
|
PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100); |
||||||
|
PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100); |
||||||
|
PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100); |
||||||
|
PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100); |
||||||
|
PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100); |
||||||
|
|
||||||
|
orb_unsubscribe(fd_status); |
||||||
|
orb_unsubscribe(fd_lpos); |
||||||
|
orb_unsubscribe(fd_gyro); |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
} // namespace MicroBenchORB
|
||||||
Loading…
Reference in new issue