yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Testing cleanup from Daniel Agar

Browse Source
This commit is contained in:
Lorenz Meier
2016-07-29 13:27:58 +02:00
parent 99aa5f49fc
commit 6ab9dc0acf
56 changed files with 1930 additions and 3416 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -246,7 +246,7 @@ endforeach()
px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg") px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg")
px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp") px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0") px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") px4_add_git_submodule(TARGET git_gtest PATH "unittests/gtest")
px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")

24
Makefile
View File

@@ -174,9 +174,6 @@ posix_sitl_default:
posix_sitl_lpe: posix_sitl_lpe:
$(call cmake-build,$@) $(call cmake-build,$@)
posix_sitl_test:
$(call cmake-build,$@)
posix_sitl_replay: posix_sitl_replay:
$(call cmake-build,$@) $(call cmake-build,$@)
@@ -271,14 +268,14 @@ checks_uavcan: \
check_px4fmu-v4_default_and_uavcan check_px4fmu-v4_default_and_uavcan
checks_sitls: \ checks_sitls: \
check_posix_sitl_default \ check_posix_sitl_default
check_posix_sitl_test \
checks_last: \ checks_last: \
check_unittest \ check_tests \
check_format \ check_format \
check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_sitls checks_last check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_last
quick_check: check_px4fmu-v2_default check_px4fmu-v4_default check_tests check_format
check_format: check_format:
$(call colorecho,"Checking formatting with astyle") $(call colorecho,"Checking formatting with astyle")
@@ -300,15 +297,18 @@ ifeq ($(VECTORCONTROL),1)
@rm -rf ROMFS/px4fmu_common/uavcan @rm -rf ROMFS/px4fmu_common/uavcan
endif endif
unittest: posix_sitl_test unittest: posix_sitl_default
$(call cmake-build-other,unittest, ../unittests) $(call cmake-build-other,unittest, ../unittests)
@(cd build_unittest && ctest -j2 --output-on-failure) @(cd build_unittest && ctest -j2 --output-on-failure)
tests: posix_sitl_test unittest run_tests_posix: posix_sitl_default
@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
test_onboard_sitl: @mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
@HEADLESS=1 make posix_sitl_test gazebo_iris @touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters
@(cd build_posix_sitl_default/src/firmware/posix && ./mainapp -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
@(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output)
tests: check_unittest run_tests_posix
# QGroundControl flashable firmware # QGroundControl flashable firmware
qgc_firmware: \ qgc_firmware: \

69
ROMFS/px4fmu_test/init.d/rcS
View File

@@ -111,87 +111,22 @@ fi
sh /etc/init.d/rc.sensors sh /etc/init.d/rc.sensors
# Check for flow sensor ver all
if px4flow start
then
fi
if ll40ls start
then
fi
# #
# Run unit tests at board boot, reporting failure as needed. # Run unit tests at board boot, reporting failure as needed.
# Add new unit tests using the same pattern as below. # Add new unit tests using the same pattern as below.
# #
echo
echo "--------------------------------------------------------------------------------"
echo "[mavlink_tests] STARTING TEST"
if mavlink_tests
then
echo "[mavlink_tests] PASS"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
echo "[mavlink_tests] FAILED"
fi
echo "--------------------------------------------------------------------------------"
echo
echo "--------------------------------------------------------------------------------"
echo "[commander_tests] STARTING TEST"
if commander_tests
then
echo "[commander_tests] PASS"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
echo "[commander_tests] FAILED"
fi
echo "--------------------------------------------------------------------------------"
echo
echo "--------------------------------------------------------------------------------"
echo "[controllib_test] STARTING TEST"
if controllib_test
then
echo "[controllib_test] PASS"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
echo "[controllib_test] FAILED"
fi
echo "--------------------------------------------------------------------------------"
echo
echo "--------------------------------------------------------------------------------"
echo "[uorb_tests] STARTING TEST"
if uorb_tests
then
echo "[uorb_tests] PASS"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
echo "[uorb_tests] FAILED"
fi
echo "--------------------------------------------------------------------------------"
if tests all if tests all
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} system_tests"
fi
if [ $unit_test_failure == 0 ]
then then
echo echo
echo "All Unit Tests PASSED" echo "All Unit Tests PASSED"
rgbled rgb 20 255 20 rgbled rgb 20 255 20
else else
echo echo
echo "Some Unit Tests FAILED:${unit_test_failure_list}" echo "Some Unit Tests FAILED"
rgbled rgb 255 20 20 rgbled rgb 255 20 20
fi fi
ver all
free free

30
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@@ -53,22 +53,34 @@ set(config_module_list
# System commands # System commands
# #
systemcmds/bl_update systemcmds/bl_update
systemcmds/config
systemcmds/dumpfile
#systemcmds/esc_calib
systemcmds/mixer systemcmds/mixer
#systemcmds/motor_ramp
systemcmds/mtd
systemcmds/nshterm
systemcmds/param systemcmds/param
systemcmds/perf systemcmds/perf
systemcmds/pwm systemcmds/pwm
#systemcmds/esc_calib
systemcmds/reboot systemcmds/reboot
#systemcmds/topic_listener
systemcmds/top
systemcmds/config
systemcmds/nshterm
systemcmds/mtd
systemcmds/dumpfile
systemcmds/ver
#systemcmds/sd_bench #systemcmds/sd_bench
systemcmds/top
#systemcmds/topic_listener
systemcmds/ver
#
# Testing
#
#drivers/sf0x/sf0x_tests
#drivers/test_ppm
#lib/rc/rc_tests
#modules/commander/commander_tests
#modules/controllib_test
#modules/mavlink/mavlink_tests
#modules/unit_test
#modules/uORB/uORB_tests
#systemcmds/tests #systemcmds/tests
#systemcmds/motor_ramp
# #
# General system control # General system control

25
cmake/configs/nuttx_px4fmu-v2_test.cmake
View File

@@ -53,32 +53,34 @@ set(config_module_list
# System commands # System commands
# #
systemcmds/bl_update systemcmds/bl_update
systemcmds/config
systemcmds/dumpfile
#systemcmds/esc_calib
systemcmds/mixer systemcmds/mixer
systemcmds/motor_ramp
systemcmds/mtd
systemcmds/nshterm
systemcmds/param systemcmds/param
systemcmds/perf systemcmds/perf
systemcmds/pwm systemcmds/pwm
systemcmds/esc_calib
systemcmds/reboot systemcmds/reboot
#systemcmds/topic_listener
systemcmds/top
systemcmds/config
systemcmds/nshterm
systemcmds/mtd
systemcmds/dumpfile
systemcmds/ver
#systemcmds/sd_bench #systemcmds/sd_bench
systemcmds/tests systemcmds/top
systemcmds/motor_ramp #systemcmds/topic_listener
systemcmds/ver
# #
# Testing # Testing
# #
drivers/sf0x/sf0x_tests
drivers/test_ppm drivers/test_ppm
#lib/rc/rc_tests
modules/commander/commander_tests modules/commander/commander_tests
modules/controllib_test modules/controllib_test
modules/mavlink/mavlink_tests modules/mavlink/mavlink_tests
modules/unit_test modules/unit_test
modules/uORB/uORB_tests modules/uORB/uORB_tests
systemcmds/tests
# #
# General system control # General system control
@@ -95,9 +97,10 @@ set(config_module_list
# Estimation modules (EKF/ SO3 / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
modules/attitude_estimator_q modules/attitude_estimator_q
modules/ekf_att_pos_estimator #modules/ekf_att_pos_estimator
modules/position_estimator_inav modules/position_estimator_inav
modules/local_position_estimator modules/local_position_estimator
modules/ekf2
# #
# Vehicle Control # Vehicle Control

13
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@@ -69,9 +69,20 @@ set(config_module_list
systemcmds/dumpfile systemcmds/dumpfile
systemcmds/ver systemcmds/ver
systemcmds/sd_bench systemcmds/sd_bench
systemcmds/tests
systemcmds/motor_ramp systemcmds/motor_ramp
#
# Testing
#
drivers/sf0x/sf0x_tests
drivers/test_ppm
modules/commander/commander_tests
modules/controllib_test
modules/mavlink/mavlink_tests
modules/unit_test
modules/uORB/uORB_tests
systemcmds/tests
# #
# General system control # General system control
# #

5
cmake/configs/posix_sitl_default.cmake
View File

@@ -70,6 +70,7 @@ set(config_module_list
lib/launchdetection lib/launchdetection
lib/mathlib lib/mathlib
lib/mathlib/math/filter lib/mathlib/math/filter
lib/rc
lib/runway_takeoff lib/runway_takeoff
lib/tailsitter_recovery lib/tailsitter_recovery
lib/terrain_estimation lib/terrain_estimation
@@ -79,9 +80,11 @@ set(config_module_list
# #
# Testing # Testing
# #
drivers/sf0x/sf0x_tests
lib/rc/rc_tests
modules/commander/commander_tests modules/commander/commander_tests
modules/controllib_test modules/controllib_test
#modules/mavlink/mavlink_tests #modules/mavlink/mavlink_tests #TODO: fix mavlink_tests
modules/unit_test modules/unit_test
modules/uORB/uORB_tests modules/uORB/uORB_tests
systemcmds/tests systemcmds/tests

110
cmake/configs/posix_sitl_test.cmake
View File

@@ -1,110 +0,0 @@
include(posix/px4_impl_posix)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
set(config_module_list
drivers/boards/sitl
drivers/device
drivers/gps
drivers/pwm_out_sim
platforms/common
platforms/posix/drivers/accelsim
platforms/posix/drivers/adcsim
platforms/posix/drivers/airspeedsim
platforms/posix/drivers/barosim
platforms/posix/drivers/gpssim
platforms/posix/drivers/gyrosim
platforms/posix/drivers/ledsim
platforms/posix/drivers/rgbledsim
platforms/posix/drivers/tonealrmsim
platforms/posix/px4_layer
platforms/posix/work_queue
systemcmds/esc_calib
systemcmds/mixer
systemcmds/param
systemcmds/perf
systemcmds/reboot
systemcmds/sd_bench
systemcmds/topic_listener
systemcmds/ver
modules/attitude_estimator_ekf
modules/attitude_estimator_q
modules/commander
modules/dataman
modules/ekf2
modules/ekf_att_pos_estimator
modules/fw_att_control
modules/fw_pos_control_l1
modules/land_detector
modules/logger
modules/mavlink
modules/mc_att_control
modules/mc_att_control_multiplatform
modules/mc_pos_control
modules/mc_pos_control_multiplatform
modules/navigator
modules/param
modules/position_estimator_inav
modules/local_position_estimator
modules/sdlog2
modules/sensors
modules/simulator
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/vtol_att_control
lib/controllib
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/external_lgpl
lib/geo
lib/geo_lookup
lib/launchdetection
lib/mathlib
lib/mathlib/math/filter
lib/runway_takeoff
lib/tailsitter_recovery
lib/terrain_estimation
examples/px4_simple_app
#
# Testing
#
modules/commander/commander_tests
modules/controllib_test
#modules/mavlink/mavlink_tests
modules/unit_test
modules/uORB/uORB_tests
systemcmds/tests
)
set(config_extra_builtin_cmds
serdis
sercon
)
set(config_sitl_rcS
posix-configs/SITL/init/rcS_test
CACHE FILEPATH "init script for sitl"
)
set(config_sitl_viewer
jmavsim
CACHE STRING "viewer for sitl"
)
set_property(CACHE config_sitl_viewer
PROPERTY STRINGS "jmavsim;none")
set(config_sitl_debugger
disable
CACHE STRING "debugger for sitl"
)
set_property(CACHE config_sitl_debugger
PROPERTY STRINGS "disable;gdb;lldb")

25
posix-configs/SITL/init/rcS_test_gazebo_iris
View File

@@ -1,25 +0,0 @@
uorb start
param load
dataman start
simulator start -s
rgbledsim start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander_tests
controllib_test
uorb_tests
tests all
ver all
shutdown

25
posix-configs/SITL/init/rcS_test_jmavsim_iris
View File

@@ -1,25 +0,0 @@
uorb start
param load
dataman start
simulator start -s
rgbledsim start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander_tests
controllib_test
uorb_tests
tests all
ver all
shutdown

12
posix-configs/SITL/init/rcS_tests Normal file
View File

@@ -0,0 +1,12 @@
uorb start
param load
param set SYS_RESTART_TYPE 0
dataman start
ver all
sleep 1
tests all
shutdown

43
src/drivers/sf0x/sf0x_tests/CMakeLists.txt Normal file
View File

@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
px4_add_module(
MODULE drivers__sf0x__sf0x_tests
MAIN sf0x_tests
COMPILE_FLAGS
SRCS
SF0XTest.cpp
../sf0x_parser.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

53
unittests/sf0x_test.cpp → src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
View File

@@ -1,19 +1,35 @@
#include <stdio.h> #include <unit_test/unit_test.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <drivers/sf0x/sf0x_parser.h> #include <drivers/sf0x/sf0x_parser.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include "gtest/gtest.h" #include <stdio.h>
#include <unistd.h>
TEST(SF0XTest, SF0X) extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);
class SF0XTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool sf0xTest();
};
bool SF0XTest::run_tests(void)
{
ut_run_test(sf0xTest);
return (_tests_failed == 0);
}
bool SF0XTest::sf0xTest(void)
{ {
const char _LINE_MAX = 20; const char _LINE_MAX = 20;
char _linebuf[_LINE_MAX]; //char _linebuf[_LINE_MAX];
_linebuf[0] = '\0'; //_linebuf[0] = '\0';
const char *lines[] = {"0.01\r\n", const char *lines[] = {"0.01\r\n",
"0.02\r\n", "0.02\r\n",
@@ -39,7 +55,6 @@ TEST(SF0XTest, SF0X)
unsigned _parsebuf_index = 0; unsigned _parsebuf_index = 0;
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
//printf("\n%s", _linebuf); //printf("\n%s", _linebuf);
int parse_ret; int parse_ret;
@@ -48,6 +63,19 @@ TEST(SF0XTest, SF0X)
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
if (parse_ret == 0) { if (parse_ret == 0) {
if (l == 0) {
ut_test(dist_m - 0.010000f < 0.001f);
} else if (l == 1) {
ut_test(dist_m - 0.020000f < 0.001f);
} else if (l == 2) {
ut_test(dist_m - 0.030000f < 0.001f);
} else if (l == 3) {
ut_test(dist_m - 0.040000f < 0.001f);
}
//printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); //printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
} }
} }
@@ -56,5 +84,8 @@ TEST(SF0XTest, SF0X)
} }
warnx("test finished"); return true;
} }
ut_declare_test_c(sf0x_tests_main, SF0XTest)

1
src/lib/rc/CMakeLists.txt
View File

@@ -34,6 +34,7 @@ px4_add_module(
MODULE lib__rc MODULE lib__rc
COMPILE_FLAGS COMPILE_FLAGS
-Os -Os
-Wno-unused-result
SRCS SRCS
st24.c st24.c
sumd.c sumd.c

43
src/lib/rc/rc_tests/CMakeLists.txt Normal file
View File

@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
px4_add_module(
MODULE lib__rc__rc_tests
MAIN rc_tests
COMPILE_FLAGS
-Wno-unused-result
SRCS
RCTest.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

306
src/lib/rc/rc_tests/RCTest.cpp Normal file
View File

@@ -0,0 +1,306 @@
#include <unit_test/unit_test.h>
#include <systemlib/err.h>
#include <stdio.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#define DSM_DEBUG
#include <lib/rc/sbus.h>
#include <lib/rc/dsm.h>
#include <lib/rc/st24.h>
#include <lib/rc/sumd.h>
#if !defined(CONFIG_ARCH_BOARD_SITL)
#define TEST_DATA_PATH "/fs/microsd"
#else
#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/"
#endif
extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
class RCTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool dsmTest();
bool sbus2Test();
bool st24Test();
bool sumdTest();
};
bool RCTest::run_tests(void)
{
ut_run_test(dsmTest);
ut_run_test(sbus2Test);
ut_run_test(st24Test);
ut_run_test(sumdTest);
return (_tests_failed == 0);
}
bool RCTest::dsmTest(void)
{
const char *filepath = TEST_DATA_PATH "dsm_x_data.txt";
FILE *fp;
fp = fopen(filepath, "rt");
ut_test(fp != nullptr);
//warnx("loading data from: %s", filepath);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
uint8_t frame[20];
uint16_t rc_values[18];
uint16_t num_values;
bool dsm_11_bit;
unsigned dsm_frame_drops = 0;
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
int rate_limiter = 0;
unsigned last_drop = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ut_test(ret > 0);
frame[0] = x;
unsigned len = 1;
// Pipe the data into the parser
bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values,
&dsm_11_bit, &dsm_frame_drops, max_channels);
if (result) {
//warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
for (unsigned i = 0; i < num_values; i++) {
//printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
}
}
if (last_drop != (dsm_frame_drops)) {
//warnx("frame dropped, now #%d", (dsm_frame_drops));
last_drop = dsm_frame_drops;
}
rate_limiter++;
}
ut_test(ret == EOF);
return true;
}
bool RCTest::sbus2Test(void)
{
const char *filepath = TEST_DATA_PATH "sbus2_r7008SB.txt";
FILE *fp;
fp = fopen(filepath, "rt");
ut_test(fp != nullptr);
//warnx("loading data from: %s", filepath);
// if (argc < 2)
// errx(1, "Need a filename for the input file");
//int byte_offset = 7;
// if (argc > 2) {
// char* end;
// byte_offset = strtol(argv[2],&end,10);
// }
//warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
uint8_t frame[SBUS_BUFFER_SIZE];
uint16_t rc_values[18];
uint16_t num_values;
unsigned sbus_frame_drops = 0;
unsigned sbus_frame_resets = 0;
bool sbus_failsafe;
bool sbus_frame_drop;
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
int rate_limiter = 0;
unsigned last_drop = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ut_test(ret > 0);
frame[0] = x;
unsigned len = 1;
// Pipe the data into the parser
hrt_abstime now = hrt_absolute_time();
// if (rate_limiter % byte_offset == 0) {
bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
&sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
if (result) {
//warnx("decoded packet");
}
// }
if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
last_drop = sbus_frame_drops + sbus_frame_resets;
}
rate_limiter++;
}
ut_test(ret == EOF);
return true;
}
bool RCTest::st24Test(void)
{
const char *filepath = TEST_DATA_PATH "st24_data.txt";
//warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
ut_test(fp != nullptr);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ut_test(ret > 0);
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[20];
if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ut_test(ret == EOF);
return true;
}
bool RCTest::sumdTest(void)
{
const char *filepath = TEST_DATA_PATH "sumd_data.txt";
//warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
ut_test(fp != nullptr);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ut_test(ret > 0);
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[32];
if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ut_test(ret == EOF);
return true;
}
ut_declare_test_c(rc_tests_main, RCTest)

0
unittests/testdata/dsm_x_data.txt → src/lib/rc/rc_tests/test_data/dsm_x_data.txt
View File

0
unittests/testdata/sbus2_r7008SB.txt → src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt
View File

0
unittests/testdata/st24_data.txt → src/lib/rc/rc_tests/test_data/st24_data.txt
View File

0
unittests/testdata/sumd_data.txt → src/lib/rc/rc_tests/test_data/sumd_data.txt
View File

2
src/modules/mavlink/mavlink_ftp.h
View File

@@ -50,7 +50,7 @@ class MavlinkFtpTest;
class MavlinkFTP : public MavlinkStream class MavlinkFTP : public MavlinkStream
{ {
public: public:
/// @brief Contructor is only public so unit test code can new objects. /// @brief Constructor is only public so unit test code can new objects.
MavlinkFTP(Mavlink *mavlink); MavlinkFTP(Mavlink *mavlink);
~MavlinkFTP(); ~MavlinkFTP();

4
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
View File

@@ -236,7 +236,7 @@ bool MavlinkFtpTest::_list_test(void)
return true; return true;
} }
/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that /// @brief Tests for correct response to a List command on a valid directory, but with an offset that
/// is beyond the last directory entry. /// is beyond the last directory entry.
bool MavlinkFtpTest::_list_eof_test(void) bool MavlinkFtpTest::_list_eof_test(void)
{ {
@@ -263,7 +263,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
return true; return true;
} }
/// @brief Tests for correct reponse to an Open command on a file which does not exist. /// @brief Tests for correct response to an Open command on a file which does not exist.
bool MavlinkFtpTest::_open_badfile_test(void) bool MavlinkFtpTest::_open_badfile_test(void)
{ {
MavlinkFTP::PayloadHeader payload; MavlinkFTP::PayloadHeader payload;

2
src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
View File

@@ -130,7 +130,7 @@ int uORBTest::UnitTest::pubsublatency_main(void)
pubsubtest_passed = true; pubsubtest_passed = true;
if (static_cast<float>(latency_integral / maxruns) > 40.0f) { if (static_cast<float>(latency_integral / maxruns) > 80.0f) {
pubsubtest_res = uORB::ERROR; pubsubtest_res = uORB::ERROR;
} else { } else {

14
src/modules/unit_test/unit_test.h
View File

@@ -37,6 +37,17 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#define ut_declare_test_c(test_function, test_class) \
extern "C" { \
int test_function(int argc, char *argv[]) \
{ \
test_class* test = new test_class(); \
bool success = test->run_tests(); \
test->print_results(); \
return success ? 0 : -1; \
} \
}
/// @brief Base class to be used for unit tests. /// @brief Base class to be used for unit tests.
class __EXPORT UnitTest class __EXPORT UnitTest
{ {
@@ -92,6 +103,9 @@ protected:
} \ } \
} while (0) } while (0)
/// @brief Used to assert a value within a unit test.
#define ut_test(test) ut_assert("test", test)
/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
/// since it will give you better error reporting of the actual values being compared. /// since it will give you better error reporting of the actual values being compared.
#define ut_compare(message, v1, v2) \ #define ut_compare(message, v1, v2) \

33
src/systemcmds/tests/CMakeLists.txt
View File

@@ -33,14 +33,26 @@
set(srcs set(srcs
test_adc.c test_adc.c
test_autodeclination.cpp
test_bson.c test_bson.c
test_float.c test_conv.cpp
test_file.c
test_file2.c
test_float.cpp
test_gpio.c test_gpio.c
test_hott_telemetry.c test_hott_telemetry.c
test_hrt.c test_hrt.c
test_int.c test_int.cpp
test_jig_voltages.c test_jig_voltages.c
test_led.c test_led.c
test_mathlib.cpp
test_matrix.cpp
test_mixer.cpp
test_mount.c
test_params.c
test_perf.c
test_ppm_loopback.c
test_rc.c
test_sensors.c test_sensors.c
test_servo.c test_servo.c
test_sleep.c test_sleep.c
@@ -48,16 +60,7 @@ set(srcs
test_uart_console.c test_uart_console.c
test_uart_loopback.c test_uart_loopback.c
test_uart_send.c test_uart_send.c
test_mixer.cpp
test_mathlib.cpp
test_file.c
test_file2.c
tests_main.c tests_main.c
test_params.c
test_ppm_loopback.c
test_rc.c
test_conv.cpp
test_mount.c
) )
if(${OS} STREQUAL "nuttx") if(${OS} STREQUAL "nuttx")
@@ -69,11 +72,15 @@ endif()
px4_add_module( px4_add_module(
MODULE systemcmds__tests MODULE systemcmds__tests
MAIN tests MAIN tests
STACK_MAIN 9000 STACK_MAIN 10000
STACK_MAX 9000 STACK_MAX 10000
COMPILE_FLAGS COMPILE_FLAGS
${MODULE_CFLAGS}
-Wno-unused-result -Wno-unused-result
-Wno-float-equal -Wno-float-equal
-Wno-missing-declarations
-Wno-double-promotion
-Wno-unknown-warning-option
-Os -Os
SRCS ${srcs} SRCS ${srcs}
DEPENDS DEPENDS

38
src/systemcmds/tests/test_autodeclination.cpp Normal file
View File

@@ -0,0 +1,38 @@
#include <unit_test/unit_test.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <px4iofirmware/px4io.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
class AutoDeclinationTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool autodeclination_check();
};
bool AutoDeclinationTest::autodeclination_check(void)
{
ut_assert("declination differs more than 1 degree", get_mag_declination(47.0, 8.0) - 0.6f < 0.5f);
return true;
}
bool AutoDeclinationTest::run_tests(void)
{
ut_run_test(autodeclination_check);
return (_tests_failed == 0);
}
ut_declare_test_c(test_autodeclination, AutoDeclinationTest)

4
src/systemcmds/tests/test_conv.cpp
View File

@@ -59,7 +59,7 @@
int test_conv(int argc, char *argv[]) int test_conv(int argc, char *argv[])
{ {
PX4_INFO("Testing system conversions"); //PX4_INFO("Testing system conversions");
for (int i = -10000; i <= 10000; i += 1) { for (int i = -10000; i <= 10000; i += 1) {
float f = i / 10000.0f; float f = i / 10000.0f;
@@ -72,7 +72,7 @@ int test_conv(int argc, char *argv[])
} }
} }
PX4_INFO("All conversions clean"); //PX4_INFO("All conversions clean");
return 0; return 0;
} }

564
src/systemcmds/tests/test_eigen.cpp
View File

@@ -1,564 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 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.
*
****************************************************************************/
/**
* @file test_eigen.cpp
*
* Eigen test (based of mathlib test)
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Nuno Marques <n.marques21@hotmail.com>
*/
#include <cmath>
#include <px4_eigen.h>
#include <mathlib/mathlib.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
using namespace Eigen;
typedef Matrix<float, 10, 1> Vector10f;
static constexpr size_t OPERATOR_ITERATIONS = 30000;
const float min = -M_PI_F;
const float max = M_PI_F;
const float step = M_PI_F / 12;
const float epsilon_f = 1E-4;
uint8_t err_num;
#define TEST_OP(_title, _op) \
{ \
const hrt_abstime t0 = hrt_absolute_time(); \
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
_op; \
} \
printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
}
#define VERIFY_OP(_title, _op, __OP_TEST__) \
{ \
_op; \
if (!(__OP_TEST__)) { \
printf(_title ": Failed! (" # __OP_TEST__ ")\n"); \
++err_num; \
} \
}
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
VERIFY_OP(_title, _op, __OP_TEST__) \
TEST_OP(_title, _op)
#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \
(fabsf(q_exp.x() - q_act.x()) <= epsilon && \
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
#define EXPECT_NEAR(expected, actual, epsilon) \
(fabsf(expected - actual) <= epsilon)
/**
* @brief
* Prints an Eigen::Matrix to stdout
**/
template<typename T>
void printEigen(const Eigen::MatrixBase<T> &b)
{
for (int i = 0; i < b.rows(); ++i) {
printf("(");
for (int j = 0; j < b.cols(); ++j) {
if (j > 0) {
printf(",");
}
printf("%.3f", static_cast<double>(b(i, j)));
}
printf(")%s\n", i + 1 < b.rows() ? "," : "");
}
}
// Methods definition
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy);
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q);
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy);
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q);
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
/**
* @brief
* Construct new Eigen::Quaternion from euler angles
* Right order is YPR.
**/
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
{
return Eigen::Quaternionf(
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
}
/**
* @brief
* Construct new Eigen::Vector3f of euler angles from quaternion
* Right order is YPR.
**/
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
{
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
/**
* @brief
* Construct new Eigen::Matrix3f from euler angles
**/
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
{
return quatFromEuler(rpy).toRotationMatrix();
}
/**
* @brief
* Adjust PX4 math::quaternion to Eigen::Quaternionf
**/
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
{
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
}
/**
* @brief
* Adjust Eigen::Quaternionf to PX4 math::quaternion
**/
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
{
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
}
/**
* @brief
* Testing main routine
**/
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("Testing Eigen math...");
{
Eigen::Vector2f v;
Eigen::Vector2f v1(1.0f, 2.0f);
Eigen::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
}
{
Eigen::Vector3f v;
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector3f = Vector3f", v = v1);
TEST_OP("Vector3f + Vector3f", v + v1);
TEST_OP("Vector3f - Vector3f", v - v1);
TEST_OP("Vector3f += Vector3f", v += v1);
TEST_OP("Vector3f -= Vector3f", v -= v1);
TEST_OP("Vector3f * float", v1 * 2.0f);
TEST_OP("Vector3f / float", v1 / 2.0f);
TEST_OP("Vector3f *= float", v1 *= 2.0f);
TEST_OP("Vector3f /= float", v1 /= 2.0f);
TEST_OP("Vector3f dot Vector3f", v.dot(v1));
TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
TEST_OP("Vector3f length", v1.norm());
TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here instead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
}
{
Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
Eigen::Vector4f vres;
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
}
{
Vector10f v1;
v1.Zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Vector10f v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data));
}
{
Eigen::Matrix3f m1;
m1.setIdentity();
Eigen::Matrix3f m2;
m2.setIdentity();
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix3f * Vector3f", m1 * v1);
TEST_OP("Matrix3f + Matrix3f", m1 + m2);
TEST_OP("Matrix3f * Matrix3f", m1 * m2);
}
{
Eigen::Matrix<float, 10, 10> m1;
m1.setIdentity();
Eigen::Matrix<float, 10, 10> m2;
m2.setIdentity();
Vector10f v1;
v1.Zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
{
printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time());
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
Eigen::Matrix<float, 2, 3> m2;
m2 << 2, 4, 6,
8, 10, 12;
Eigen::Matrix<float, 2, 3> m3;
m3 << 3, 6, 9,
12, 15, 18;
if (m1 + m2 != m3) {
printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1 + m2);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
if (m3 - m2 != m1) {
printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m3 - m2);
printf("!=\n");
printEigen(m1);
++err_num;
rc = 1;
}
m1 += m2;
if (m1 != m3) {
printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
m1 -= m2;
if (m1 != m1_orig) {
printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m1_orig);
++err_num;
rc = 1;
}
}
/* QUATERNION TESTS */
{
// Test conversion rotation matrix to quaternion and back
// against existing PX4 mathlib
math::Matrix<3, 3> R_orig;
Eigen::Quaternionf q_true;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
/******************************************** TEST 1 ****************************************************/
q_true = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4));
if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q2_px4(px4qFromEigenq(q2_eigen));
Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]);
if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]);
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f};
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f};
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f};
for (size_t i = 0; i < 4; i++) {
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
(unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 6 ****************************************************/
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)));
R = q.toRotationMatrix();
R_orig.from_euler(roll, pitch, yaw);
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
(unsigned long long)hrt_absolute_time(), (int)i, (int)j);
printf("Actual: \t%8.5f\n", R(i, j));
printf("Expected: \t%8.5f\n", R_orig(i, j));
++err_num;
rc = 1;
}
}
}
}
}
}
}
{
// Test rotation method (rotate vector by quaternion)
Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
Eigen::Vector3f vector_q;
Eigen::Vector3f vector_r;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
/******************************************** TEST 1 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
vector_q = q._transformVector(vector);
vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
vector_q = q._transformVector(vector);
vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
vector_q = q._transformVector(vector);
vector_true = { -1.366030f, 0.366025f, 1.000000f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f);
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw));
q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw));
vector_r = R * vectorR;
vector_q = q._transformVector(vectorR);
for (int i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) {
printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i);
printf("Actual: \t%8.5f\n", vector_q(i));
printf("Expected: \t%8.5f\n", vector_r(i));
++err_num;
rc = 1;
}
}
}
}
}
}
printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num);
return rc;
}

287
src/systemcmds/tests/test_float.c
View File

@@ -1,287 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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.
*
****************************************************************************/
/**
* @file tests_float.c
* Floating point tests
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests.h"
#include <math.h>
#include <float.h>
typedef union {
float f;
double d;
uint8_t b[8];
} test_float_double_t;
int test_float(int argc, char *argv[])
{
int ret = 0;
printf("\n--- SINGLE PRECISION TESTS ---\n");
printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
float f1 = 1.55f;
float sinf_zero = sinf(0.0f);
float sinf_one = sinf(1.0f);
float sqrt_two = sqrt(2.0f);
if (fabsf(sinf_zero) < FLT_EPSILON) {
printf("\t success: sinf(0.0f) == 0.0f\n");
} else {
printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
ret = -4;
}
fflush(stdout);
if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
printf("\t success: sinf(1.0f) == 0.84147f\n");
} else {
printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
ret = -1;
}
fflush(stdout);
float asinf_one = asinf(1.0f);
if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
printf("\t success: asinf(1.0f) == 1.57079f\n");
} else {
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
ret = -1;
}
fflush(stdout);
float cosf_one = cosf(1.0f);
if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
printf("\t success: cosf(1.0f) == 0.54030f\n");
} else {
printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
ret = -1;
}
fflush(stdout);
float acosf_one = acosf(1.0f);
if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
printf("\t success: acosf(1.0f) == 0.0f\n");
} else {
printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
ret = -1;
}
fflush(stdout);
float sinf_zero_one = sinf(0.1f);
if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
printf("\t success: sinf(0.1f) == 0.09983f\n");
} else {
printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
ret = -2;
}
if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
printf("\t success: sqrt(2.0f) == 1.41421f\n");
} else {
printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
ret = -3;
}
float atan2f_ones = atan2f(1.0f, 1.0f);
if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON) {
printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
} else {
printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
ret = -4;
}
char sbuf[30];
sprintf(sbuf, "%8.4f", (double)0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
} else {
printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \" 0.5534\", result: %s\n", sbuf);
ret = -5;
}
sprintf(sbuf, "%8.4f", (double) - 0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double) - 0.553415f);
} else {
printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
ret = -6;
}
printf("\n--- DOUBLE PRECISION TESTS ---\n");
double d1 = 1.0111;
double d2 = 2.0;
double d1d2 = d1 * d2;
if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
printf("\t success: 1.0111 * 2.0 == 2.0222\n");
} else {
printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
ret = -7;
}
fflush(stdout);
// Assign value of f1 to d1
d1 = f1;
if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
printf("\t success: (float) 1.55f == 1.55 (double)\n");
} else {
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
ret = -8;
}
fflush(stdout);
double sin_zero = sin(0.0);
double sin_one = sin(1.0);
double atan2_ones = atan2(1.0, 1.0);
if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
printf("\t success: sin(0.0) == 0.0\n");
} else {
printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
ret = -9;
}
if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
printf("\t success: sin(1.0) == 0.84147098480\n");
} else {
printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
ret = -10;
}
if (fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON) {
printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
} else {
printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
ret = -11;
}
printf("\t testing pow() with magic value\n");
printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
fflush(stdout);
usleep(20000);
double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
printf("\t success: result: %8.4f\n", (double)powres);
sprintf(sbuf, "%8.4f", 0.553415);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
} else {
printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \" 0.5534\", result: %s\n", sbuf);
ret = -12;
}
sprintf(sbuf, "%8.4f", -0.553415);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
} else {
printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
ret = -13;
}
if (ret == 0) {
printf("\n SUCCESS: All float and double tests passed.\n");
} else {
printf("\n FAIL: One or more tests failed.\n");
}
printf("\n");
return ret;
}

149
src/systemcmds/tests/test_float.cpp Normal file
View File

@@ -0,0 +1,149 @@
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
typedef union {
float f;
double d;
uint8_t b[8];
} test_float_double_t;
class FloatTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool singlePrecisionTests();
bool doublePrecisionTests();
};
bool FloatTest::singlePrecisionTests(void)
{
float sinf_zero = sinf(0.0f);
float sinf_one = sinf(1.0f);
float sqrt_two = sqrt(2.0f);
ut_assert("sinf(0.0f) == 0.0f", fabsf(sinf_zero) < FLT_EPSILON);
ut_assert("sinf(1.0f) == 0.84147f", fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON);
float asinf_one = asinf(1.0f);
ut_assert("asinf(1.0f) == 1.57079f", fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f);
float cosf_one = cosf(1.0f);
ut_assert("cosf(1.0f) == 0.54030f", fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON);
float acosf_one = acosf(1.0f);
ut_assert("acosf(1.0f) == 0.0f", fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON);
float sinf_zero_one = sinf(0.1f);
ut_assert("sinf(0.1f) == 0.09983f", fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON);
ut_assert("sqrt(2.0f) == 1.41421f", fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON);
float atan2f_ones = atan2f(1.0f, 1.0f);
ut_assert("atan2f(1.0f, 1.0f) == 0.78539f",
fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON);
char sbuf[30];
sprintf(sbuf, "%8.4f", (double)0.553415f);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], ' ');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
sprintf(sbuf, "%8.4f", (double) - 0.553415f);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], '-');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
return true;
}
bool FloatTest::doublePrecisionTests(void)
{
float f1 = 1.55f;
double d1 = 1.0111;
double d2 = 2.0;
double d1d2 = d1 * d2;
ut_assert("1.0111 * 2.0 == 2.0222", fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON);
// Assign value of f1 to d1
d1 = f1;
ut_assert("(float) 1.55f == 1.55 (double)", fabsf(f1 - (float)d1) < FLT_EPSILON);
double sin_zero = sin(0.0);
double sin_one = sin(1.0);
double atan2_ones = atan2(1.0, 1.0);
ut_assert("sin(0.0) == 0.0", fabs(sin_zero - 0.0) < DBL_EPSILON);
ut_assert("sin(1.0) == 0.84147098480", fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON);
ut_assert("atan2(1.0, 1.0) == 0.785398", fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON);
ut_assert("testing pow() with magic value",
(44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))) - 428.2293 < DBL_EPSILON);
char sbuf[30];
sprintf(sbuf, "%8.4f", 0.553415);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], ' ');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
sprintf(sbuf, "%8.4f", -0.553415);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], '-');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
return true;
}
bool FloatTest::run_tests(void)
{
ut_run_test(singlePrecisionTests);
ut_run_test(doublePrecisionTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_float, FloatTest)

152
src/systemcmds/tests/test_int.c
View File

@@ -1,152 +0,0 @@
/****************************************************************************
* px4/sensors/test_gpio.c
*
* Copyright (C) 2012 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <inttypes.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <arch/board/board.h>
#include "tests.h"
#include <math.h>
#include <float.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
typedef union {
int32_t i;
int64_t l;
uint8_t b[8];
} test_32_64_t;
int test_int(int argc, char *argv[])
{
int ret = 0;
printf("\n--- 64 BIT MATH TESTS ---\n");
int64_t large = 354156329598;
int64_t calc = large * 5;
if (calc == 1770781647990) {
printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc);
} else {
printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc);
ret = -1;
}
fflush(stdout);
printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
int32_t small = 50;
int32_t large_int = 2147483647; // MAX INT value
uint64_t small_times_large = large_int * (uint64_t)small;
if (small_times_large == 107374182350) {
printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large);
} else {
printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large);
ret = -1;
}
fflush(stdout);
if (ret == 0) {
printf("\n SUCCESS: All float and double tests passed.\n");
} else {
printf("\n FAIL: One or more tests failed.\n");
}
printf("\n");
return ret;
}

61
src/systemcmds/tests/test_int.cpp Normal file
View File

@@ -0,0 +1,61 @@
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
typedef union {
int32_t i;
int64_t l;
uint8_t b[8];
} test_32_64_t;
class IntTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool math64bitTests();
bool math3264MixedMathTests();
};
bool IntTest::math64bitTests(void)
{
int64_t large = 354156329598;
int64_t calc = large * 5;
ut_assert("354156329598 * 5 == 1770781647990", calc == 1770781647990);
return true;
}
bool IntTest::math3264MixedMathTests(void)
{
int32_t small = 50;
int32_t large_int = 2147483647; // MAX INT value
uint64_t small_times_large = large_int * (uint64_t)small;
ut_assert("64bit calculation: 50 * 2147483647 (max int val) == 107374182350", small_times_large == 107374182350);
return true;
}
bool IntTest::run_tests(void)
{
ut_run_test(math64bitTests);
ut_run_test(math3264MixedMathTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_int, IntTest)

404
src/systemcmds/tests/test_mathlib.cpp
View File

@@ -31,11 +31,18 @@
* *
****************************************************************************/ ****************************************************************************/
/** #include <unit_test/unit_test.h>
* @file test_mathlib.cpp
* #include <errno.h>
* Mathlib test #include <fcntl.h>
*/ #include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <px4_log.h> #include <px4_log.h>
#include <stdio.h> #include <stdio.h>
@@ -47,17 +54,33 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
class MathlibTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool testVector2();
bool testVector3();
bool testVector4();
bool testVector10();
bool testMatrix3x3();
bool testMatrix10x10();
bool testMatrixNonsymmetric();
bool testRotationMatrixQuaternion();
bool testQuaternionfrom_dcm();
bool testQuaternionfrom_euler();
bool testQuaternionRotate();
};
#include "tests.h" #include "tests.h"
#define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); } #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
using namespace math; using namespace math;
int test_mathlib(int argc, char *argv[]) bool MathlibTest::testVector2(void)
{ {
int rc = 0;
PX4_INFO("testing mathlib");
{ {
Vector<2> v; Vector<2> v;
Vector<2> v1(1.0f, 2.0f); Vector<2> v1(1.0f, 2.0f);
@@ -75,6 +98,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<2> * Vector<2>", v * v1); TEST_OP("Vector<2> * Vector<2>", v * v1);
TEST_OP("Vector<2> %% Vector<2>", v1 % v2); TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
} }
return true;
}
bool MathlibTest::testVector3(void)
{
{ {
Vector<3> v; Vector<3> v;
@@ -108,7 +136,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
} }
return true;
}
bool MathlibTest::testVector4(void)
{
{ {
Vector<4> v; Vector<4> v;
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
@@ -125,7 +157,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<4> -= Vector<4>", v -= v1); TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> * Vector<4>", v * v1); TEST_OP("Vector<4> * Vector<4>", v * v1);
} }
return true;
}
bool MathlibTest::testVector10(void)
{
{ {
Vector<10> v1; Vector<10> v1;
v1.zero(); v1.zero();
@@ -134,7 +170,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
} }
return true;
}
bool MathlibTest::testMatrix3x3(void)
{
{ {
Matrix<3, 3> m1; Matrix<3, 3> m1;
m1.identity(); m1.identity();
@@ -145,7 +185,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
} }
return true;
}
bool MathlibTest::testMatrix10x10(void)
{
{ {
Matrix<10, 10> m1; Matrix<10, 10> m1;
m1.identity(); m1.identity();
@@ -157,9 +201,14 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
} }
return true;
}
bool MathlibTest::testMatrixNonsymmetric(void)
{
int rc = true;
{ {
PX4_INFO("Nonsymmetric matrix operations test"); //PX4_INFO("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -= // test nonsymmetric +, -, +=, -=
float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
(m1 + m2).print(); (m1 + m2).print();
printf("!=\n"); printf("!=\n");
m3.print(); m3.print();
rc = 1; rc = false;
} }
ut_assert("m1 + m2 == m3", m1 + m2 == m3);
if (m3 - m2 != m1) { if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!"); PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
(m3 - m2).print(); (m3 - m2).print();
printf("!=\n"); printf("!=\n");
m1.print(); m1.print();
rc = 1; rc = false;
} }
ut_assert("m3 - m2 == m1", m3 - m2 == m1);
m1 += m2; m1 += m2;
if (m1 != m3) { if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
m1.print(); m1.print();
printf("!=\n"); printf("!=\n");
m3.print(); m3.print();
rc = 1; rc = false;
} }
ut_assert("m1 == m3", m1 == m3);
m1 -= m2; m1 -= m2;
Matrix<2, 3> m1_orig(data1); Matrix<2, 3> m1_orig(data1);
@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
m1.print(); m1.print();
printf("!=\n"); printf("!=\n");
m1_orig.print(); m1_orig.print();
rc = 1; rc = false;
} }
} ut_assert("m1 == m1_orig", m1 == m1_orig);
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
R_orig.identity();
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
} }
return rc; return rc;
} }
bool MathlibTest::testRotationMatrixQuaternion(void)
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
}
}
}
}
}
return true;
}
bool MathlibTest::testQuaternionfrom_dcm(void)
{
// test against some known values
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionfrom_euler(void)
{
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionRotate(void)
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
return true;
}
bool MathlibTest::run_tests(void)
{
ut_run_test(testVector2);
ut_run_test(testVector3);
ut_run_test(testVector4);
ut_run_test(testVector10);
ut_run_test(testMatrix3x3);
ut_run_test(testMatrix10x10);
ut_run_test(testMatrixNonsymmetric);
ut_run_test(testRotationMatrixQuaternion);
ut_run_test(testQuaternionfrom_dcm);
ut_run_test(testQuaternionfrom_euler);
ut_run_test(testQuaternionRotate);
return (_tests_failed == 0);
}
ut_declare_test_c(test_mathlib, MathlibTest)

679
src/systemcmds/tests/test_matrix.cpp Normal file
View File

@@ -0,0 +1,679 @@
#include <unit_test/unit_test.h>
#include <matrix/math.hpp>
#include <matrix/filter.hpp>
#include <matrix/integration.hpp>
using namespace matrix;
class MatrixTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool attitudeTests();
bool filterTests();
bool helperTests();
bool integrationTests();
bool inverseTests();
bool matrixAssignmentTests();
bool matrixMultTests();
bool matrixScalarMultTests();
bool setIdentityTests();
bool sliceTests();
bool squareMatrixTests();
bool transposeTests();
bool vectorTests();
bool vector2Tests();
bool vector3Tests();
bool vectorAssignmentTests();
};
bool MatrixTest::run_tests(void)
{
ut_run_test(attitudeTests);
ut_run_test(filterTests);
ut_run_test(helperTests);
ut_run_test(integrationTests);
ut_run_test(inverseTests);
ut_run_test(matrixAssignmentTests);
ut_run_test(matrixMultTests);
ut_run_test(matrixScalarMultTests);
ut_run_test(setIdentityTests);
ut_run_test(sliceTests);
ut_run_test(squareMatrixTests);
ut_run_test(transposeTests);
ut_run_test(vectorTests);
ut_run_test(vector2Tests);
ut_run_test(vector3Tests);
ut_run_test(vectorAssignmentTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_matrix, MatrixTest)
template class matrix::Quaternion<float>;
template class matrix::Euler<float>;
template class matrix::Dcm<float>;
bool MatrixTest::attitudeTests(void)
{
double eps = 1e-6;
// check data
Eulerf euler_check(0.1f, 0.2f, 0.3f);
Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
float dcm_data[] = {
0.93629336f, -0.27509585f, 0.21835066f,
0.28962948f, 0.95642509f, -0.03695701f,
-0.19866933f, 0.0978434f, 0.97517033f
};
Dcmf dcm_check(dcm_data);
// euler ctor
ut_test(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
// euler default ctor
Eulerf e;
Eulerf e_zero = zeros<float, 3, 1>();
ut_test(isEqual(e, e_zero));
ut_test(isEqual(e, e));
// euler vector ctor
Vector<float, 3> v;
v(0) = 0.1f;
v(1) = 0.2f;
v(2) = 0.3f;
Eulerf euler_copy(v);
ut_test(isEqual(euler_copy, euler_check));
// quaternion ctor
Quatf q0(1, 2, 3, 4);
Quatf q(q0);
ut_test(fabs(q(0) - 1) < eps);
ut_test(fabs(q(1) - 2) < eps);
ut_test(fabs(q(2) - 3) < eps);
ut_test(fabs(q(3) - 4) < eps);
// quat normalization
q.normalize();
ut_test(isEqual(q, Quatf(0.18257419f, 0.36514837f,
0.54772256f, 0.73029674f)));
ut_test(isEqual(q0.unit(), q));
// quat default ctor
q = Quatf();
ut_test(isEqual(q, Quatf(1, 0, 0, 0)));
// euler to quaternion
q = Quatf(euler_check);
ut_test(isEqual(q, q_check));
// euler to dcm
Dcmf dcm(euler_check);
ut_test(isEqual(dcm, dcm_check));
// quaternion to euler
Eulerf e1(q_check);
ut_test(isEqual(e1, euler_check));
// quaternion to dcm
Dcmf dcm1(q_check);
ut_test(isEqual(dcm1, dcm_check));
// dcm default ctor
Dcmf dcm2;
SquareMatrix<float, 3> I = eye<float, 3>();
ut_test(isEqual(dcm2, I));
// dcm to euler
Eulerf e2(dcm_check);
ut_test(isEqual(e2, euler_check));
// dcm to quaterion
Quatf q2(dcm_check);
ut_test(isEqual(q2, q_check));
// constants
double deg2rad = M_PI / 180.0;
double rad2deg = 180.0 / M_PI;
// euler dcm round trip check
for (int roll = -90; roll <= 90; roll += 90) {
for (int pitch = -90; pitch <= 90; pitch += 90) {
for (int yaw = -179; yaw <= 180; yaw += 90) {
// note if theta = pi/2, then roll is set to zero
int roll_expected = roll;
int yaw_expected = yaw;
if (pitch == 90) {
roll_expected = 0;
yaw_expected = yaw - roll;
} else if (pitch == -90) {
roll_expected = 0;
yaw_expected = yaw + roll;
}
if (yaw_expected < -180) { yaw_expected += 360; }
if (yaw_expected > 180) { yaw_expected -= 360; }
//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler_expected(
deg2rad * double(roll_expected),
deg2rad * double(pitch),
deg2rad * double(yaw_expected));
Euler<double> euler(
deg2rad * double(roll),
deg2rad * double(pitch),
deg2rad * double(yaw));
Dcm<double> dcm_from_euler(euler);
//dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler);
ut_test(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw_expected));
Eulerf eulerf(float(deg2rad)*float(roll),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw));
Dcm<float> dcm_from_eulerf(eulerf);
Euler<float> euler_outf(dcm_from_eulerf);
ut_test(isEqual(float(rad2deg)*eulerf_expected,
float(rad2deg)*euler_outf));
}
}
}
// quaterion copy ctors
float data_v4[] = {1, 2, 3, 4};
Vector<float, 4> v4(data_v4);
Quatf q_from_v(v4);
ut_test(isEqual(q_from_v, v4));
Matrix<float, 4, 1> m4(data_v4);
Quatf q_from_m(m4);
ut_test(isEqual(q_from_m, m4));
// quaternion derivate
Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
// quaternion product
Quatf q_prod_check(
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
ut_test(isEqual(q_prod_check, q_check * q_check));
q_check *= q_check;
ut_test(isEqual(q_prod_check, q_check));
// Quaternion scalar multiplication
float scalar = 0.5;
Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
3.0f * scalar, 4.0f * scalar);
Quatf q_scalar_mul_res = scalar * q_scalar_mul;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res));
Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
Quatf q_scalar_mul_res3(q_scalar_mul);
q_scalar_mul_res3 *= scalar;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
// quaternion inverse
q = q_check.inversed();
ut_test(fabs(q_check(0) - q(0)) < eps);
ut_test(fabs(q_check(1) + q(1)) < eps);
ut_test(fabs(q_check(2) + q(2)) < eps);
ut_test(fabs(q_check(3) + q(3)) < eps);
q = q_check;
q.invert();
ut_test(fabs(q_check(0) - q(0)) < eps);
ut_test(fabs(q_check(1) + q(1)) < eps);
ut_test(fabs(q_check(2) + q(2)) < eps);
ut_test(fabs(q_check(3) + q(3)) < eps);
// rotate quaternion (nonzero rotation)
Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
Vector<float, 3> rot;
rot(0) = 1.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
ut_test(fabs(qI(0) - q_true(0)) < eps);
ut_test(fabs(qI(1) - q_true(1)) < eps);
ut_test(fabs(qI(2) - q_true(2)) < eps);
ut_test(fabs(qI(3) - q_true(3)) < eps);
// rotate quaternion (zero rotation)
qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot(0) = 0.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
ut_test(fabs(qI(0) - q_true(0)) < eps);
ut_test(fabs(qI(1) - q_true(1)) < eps);
ut_test(fabs(qI(2) - q_true(2)) < eps);
ut_test(fabs(qI(3) - q_true(3)) < eps);
// get rotation axis from quaternion (nonzero rotation)
q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
rot = q.to_axis_angle();
ut_test(fabs(rot(0)) < eps);
ut_test(fabs(rot(1) - 1.0f) < eps);
ut_test(fabs(rot(2)) < eps);
// get rotation axis from quaternion (zero rotation)
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot = q.to_axis_angle();
ut_test(fabs(rot(0)) < eps);
ut_test(fabs(rot(1)) < eps);
ut_test(fabs(rot(2)) < eps);
// from axis angle (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f;
q.from_axis_angle(rot, 0.0f);
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
ut_test(fabs(q(0) - q_true(0)) < eps);
ut_test(fabs(q(1) - q_true(1)) < eps);
ut_test(fabs(q(2) - q_true(2)) < eps);
ut_test(fabs(q(3) - q_true(3)) < eps);
return true;
}
bool MatrixTest::filterTests(void)
{
const size_t n_x = 6;
const size_t n_y = 5;
SquareMatrix<float, n_x> P = eye<float, n_x>();
SquareMatrix<float, n_y> R = eye<float, n_y>();
Matrix<float, n_y, n_x> C;
C.setIdentity();
float data[] = {1, 2, 3, 4, 5};
Vector<float, n_y> r(data);
Vector<float, n_x> dx;
SquareMatrix<float, n_x> dP;
float beta = 0;
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
Vector<float, n_x> dx_check(data_check);
ut_test(isEqual(dx, dx_check));
return true;
}
bool MatrixTest::helperTests(void)
{
ut_test(fabs(wrap_pi(4.0) - (4.0 - 2 * M_PI)) < 1e-5);
ut_test(fabs(wrap_pi(-4.0) - (-4.0 + 2 * M_PI)) < 1e-5);
ut_test(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
wrap_pi(NAN);
Vector3f a(1, 2, 3);
Vector3f b(4, 5, 6);
ut_test(isEqual(a, a));
return true;
}
Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u)
{
float v = -sinf(t);
return v * ones<float, 6, 1>();
}
bool MatrixTest::integrationTests(void)
{
Vector<float, 6> y = ones<float, 6, 1>();
Vector<float, 3> u = ones<float, 3, 1>();
float t0 = 0;
float tf = 2;
float h = 0.001f;
integrate_rk4(f, y, u, t0, tf, h, y);
float v = 1 + cosf(tf) - cosf(t0);
ut_test(isEqual(y, (ones<float, 6, 1>()*v)));
return true;
}
template class matrix::SquareMatrix<float, 3>;
bool MatrixTest::inverseTests(void)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
float data_check[9] = {
-0.4f, -0.8f, 0.6f,
-0.4f, 4.2f, -2.4f,
0.6f, -2.8f, 1.6f
};
SquareMatrix<float, 3> A(data);
SquareMatrix<float, 3> A_I = inv(A);
SquareMatrix<float, 3> A_I_check(data_check);
float eps = 1e-5;
ut_test((A_I - A_I_check).abs().max() < eps);
SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
inv(zero_test);
return true;
}
bool MatrixTest::matrixAssignmentTests(void)
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
double eps = 1e-6f;
for (int i = 0; i < 9; i++) {
ut_test(fabs(data[i] - m2.data()[i]) < eps);
}
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f m3(data_times_2);
ut_test(isEqual(m, m2));
ut_test(!(m == m3));
m2 *= 2;
ut_test(m2 == m3);
m2 /= 2;
m2 -= 1;
float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
ut_test(Matrix3f(data_minus_1) == m2);
m2 += 1;
ut_test(Matrix3f(data) == m2);
m3 -= m2;
ut_test(m3 == m2);
float data_row_02_swap[9] = {
7, 8, 9,
4, 5, 6,
1, 2, 3,
};
float data_col_02_swap[9] = {
3, 2, 1,
6, 5, 4,
9, 8, 7
};
Matrix3f m4(data);
ut_test(-m4 == m4 * (-1));
m4.swapCols(0, 2);
ut_test(m4 == Matrix3f(data_col_02_swap));
m4.swapCols(0, 2);
m4.swapRows(0, 2);
ut_test(m4 == Matrix3f(data_row_02_swap));
ut_test(fabs(m4.min() - 1) < 1e-5);
Scalar<float> s;
s = 1;
ut_test(fabs(s - 1) < 1e-5);
Matrix<float, 1, 1> m5 = s;
ut_test(fabs(m5(0, 0) - s) < 1e-5);
Matrix<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 1));
m6.setCol(0, Vector2f(1, 1));
return true;
}
bool MatrixTest::matrixMultTests(void)
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
Matrix3f R = A * A_I;
ut_test(isEqual(R, I));
Matrix3f R2 = A;
R2 *= A_I;
ut_test(isEqual(R2, I));
Matrix3f A2 = eye<float, 3>() * 2;
Matrix3f B = A2.emult(A2);
Matrix3f B_check = eye<float, 3>() * 4;
ut_test(isEqual(B, B_check));
return true;
}
bool MatrixTest::matrixScalarMultTests(void)
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
ut_test(isEqual(A, A_check));
return true;
}
template class matrix::Matrix<float, 3, 3>;
bool MatrixTest::setIdentityTests(void)
{
Matrix3f A;
A.setIdentity();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
ut_test(fabs(A(i, j) - 1) < 1e-7);
} else {
ut_test(fabs(A(i, j) - 0) < 1e-7);
}
}
}
return true;
}
bool MatrixTest::sliceTests(void)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
float data_check[6] = {
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Matrix<float, 2, 3> B_check(data_check);
Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
ut_test(isEqual(B, B_check));
float data_2[4] = {
11, 12,
13, 14
};
Matrix<float, 2, 2> C(data_2);
A.set(C, 1, 1);
float data_2_check[9] = {
0, 2, 3,
4, 11, 12,
7, 13, 14
};
Matrix<float, 3, 3> D(data_2_check);
ut_test(isEqual(A, D));
return true;
}
bool MatrixTest::squareMatrixTests(void)
{
float data[9] = {1, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Vector3<float> diag_check(1, 5, 10);
ut_test(isEqual(A.diag(), diag_check));
float data_check[9] = {
1.01158503f, 0.02190432f, 0.03238144f,
0.04349195f, 1.05428524f, 0.06539627f,
0.07576783f, 0.08708946f, 1.10894048f
};
float dt = 0.01f;
SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
SquareMatrix<float, 3> eA_check(data_check);
float eps = 1e-3;
ut_test((eA - eA_check).abs().max() < eps);
return true;
}
bool MatrixTest::transposeTests(void)
{
float data[6] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[6] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
ut_test(isEqual(A_T, A_T_check));
return true;
}
bool MatrixTest::vectorTests(void)
{
float data1[] = {1, 2, 3, 4, 5};
float data2[] = {6, 7, 8, 9, 10};
Vector<float, 5> v1(data1);
ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
Vector<float, 5> v2(data2);
ut_test(fabs(v1.dot(v2) - 130.0f) < 1e-5);
v2.normalize();
Vector<float, 5> v3(v2);
ut_test(v2 == v3);
float data1_sq[] = {1, 4, 9, 16, 25};
Vector<float, 5> v4(data1_sq);
ut_test(v1 == v4.pow(0.5));
return true;
}
bool MatrixTest::vector2Tests(void)
{
Vector2f a(1, 0);
Vector2f b(0, 1);
ut_test(fabs(a % b - 1.0f) < 1e-5);
Vector2f c;
ut_test(fabs(c(0) - 0) < 1e-5);
ut_test(fabs(c(1) - 0) < 1e-5);
Matrix<float, 2, 1> d(a);
ut_test(fabs(d(0, 0) - 1) < 1e-5);
ut_test(fabs(d(1, 0) - 0) < 1e-5);
Vector2f e(d);
ut_test(fabs(e(0) - 1) < 1e-5);
ut_test(fabs(e(1) - 0) < 1e-5);
float data[] = {4, 5};
Vector2f f(data);
ut_test(fabs(f(0) - 4) < 1e-5);
ut_test(fabs(f(1) - 5) < 1e-5);
return true;
}
bool MatrixTest::vector3Tests(void)
{
Vector3f a(1, 0, 0);
Vector3f b(0, 1, 0);
Vector3f c = a.cross(b);
ut_test(c == Vector3f(0, 0, 1));
c = a % b;
ut_test(c == Vector3f(0, 0, 1));
Matrix<float, 3, 1> d(c);
Vector3f e(d);
ut_test(e == d);
float data[] = {4, 5, 6};
Vector3f f(data);
ut_test(f == Vector3f(4, 5, 6));
return true;
}
bool MatrixTest::vectorAssignmentTests(void)
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
static const float eps = 1e-7f;
ut_test(fabsf(v(0) - 1) < eps);
ut_test(fabsf(v(1) - 2) < eps);
ut_test(fabsf(v(2) - 3) < eps);
Vector3f v2(4, 5, 6);
ut_test(fabsf(v2(0) - 4) < eps);
ut_test(fabsf(v2(1) - 5) < eps);
ut_test(fabsf(v2(2) - 6) < eps);
SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
ut_test(fabsf(m(0, 0) - 1) < eps);
ut_test(fabsf(m(1, 1) - 2) < eps);
ut_test(fabsf(m(2, 2) - 3) < eps);
return true;
}

87
src/systemcmds/tests/test_mixer.cpp
View File

@@ -85,22 +85,22 @@ int test_mixer(int argc, char *argv[])
uint16_t servo_predicted[output_max]; uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0; int16_t reverse_pwm_mask = 0;
PX4_INFO("testing mixer"); //PX4_INFO("testing mixer");
#if !defined(CONFIG_ARCH_BOARD_SITL)
const char *filename = "/etc/mixers/IO_pass.mix"; const char *filename = "/etc/mixers/IO_pass.mix";
#else
const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
#endif
if (argc > 1) { //PX4_INFO("loading: %s", filename);
filename = argv[1];
}
PX4_INFO("loading: %s", filename);
char buf[2048]; char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf)); load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf); unsigned loaded = strlen(buf);
fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); //fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
/* load the mixer in chunks, like /* load the mixer in chunks, like
* in the case of a remote load, * in the case of a remote load,
@@ -114,11 +114,7 @@ int test_mixer(int argc, char *argv[])
/* load at once test */ /* load at once test */
unsigned xx = loaded; unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx); mixer_group.load_from_buf(&buf[0], xx);
PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); //ASSERT_EQ(mixer_group.count(), 8);
if (mixer_group.count() != 8) {
return 1;
}
unsigned empty_load = 2; unsigned empty_load = 2;
char empty_buf[2]; char empty_buf[2];
@@ -126,7 +122,9 @@ int test_mixer(int argc, char *argv[])
empty_buf[1] = '\0'; empty_buf[1] = '\0';
mixer_group.reset(); mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load); mixer_group.load_from_buf(&empty_buf[0], empty_load);
PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); //PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
//ASSERT_NE(empty_load, 0);
if (empty_load != 0) { if (empty_load != 0) {
return 1; return 1;
@@ -140,7 +138,7 @@ int test_mixer(int argc, char *argv[])
unsigned transmitted = 0; unsigned transmitted = 0;
PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded); //PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
while (transmitted < loaded) { while (transmitted < loaded) {
@@ -155,7 +153,7 @@ int test_mixer(int argc, char *argv[])
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length; mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0'; mixer_text[mixer_text_length] = '\0';
fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); //fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */ /* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length; unsigned resid = mixer_text_length;
@@ -163,7 +161,7 @@ int test_mixer(int argc, char *argv[])
/* if anything was parsed */ /* if anything was parsed */
if (resid != mixer_text_length) { if (resid != mixer_text_length) {
fprintf(stderr, "used %u", mixer_text_length - resid); //fprintf(stderr, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */ /* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) { if (resid > 0) {
@@ -176,7 +174,7 @@ int test_mixer(int argc, char *argv[])
transmitted += text_length; transmitted += text_length;
} }
PX4_INFO("chunked load: loaded %u mixers", mixer_group.count()); //PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) { if (mixer_group.count() != 8) {
return 1; return 1;
@@ -198,7 +196,8 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max[i] = PWM_DEFAULT_MAX; r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
} }
PX4_INFO("PRE-ARM TEST: DISABLING SAFETY"); //PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */ /* mix */
should_prearm = true; should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, NULL); mixed = mixer_group.mix(&outputs[0], output_max, NULL);
@@ -209,7 +208,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]); //fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) { if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) { if (r_page_servos[i] < r_page_servo_control_min[i]) {
@@ -233,7 +232,7 @@ int test_mixer(int argc, char *argv[])
actuator_controls[i] = 0.1f; actuator_controls[i] = 0.1f;
} }
PX4_INFO("ARMING TEST: STARTING RAMP"); //PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000; unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time(); hrt_abstime starttime = hrt_absolute_time();
@@ -250,7 +249,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]); //fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */ /* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US && if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -274,7 +273,7 @@ int test_mixer(int argc, char *argv[])
} }
} }
PX4_INFO("ARMING TEST: NORMAL OPERATION"); //PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) { for (int j = -jmax; j <= jmax; j++) {
@@ -292,7 +291,7 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max, outputs, r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit); r_page_servos, &pwm_limit);
fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max); //fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -306,7 +305,7 @@ int test_mixer(int argc, char *argv[])
} }
} }
PX4_INFO("ARMING TEST: DISARMING"); //PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time(); starttime = hrt_absolute_time();
sleepcount = 0; sleepcount = 0;
@@ -324,7 +323,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]); //fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */ /* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) { if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -337,14 +336,14 @@ int test_mixer(int argc, char *argv[])
sleepcount++; sleepcount++;
if (sleepcount % 10 == 0) { if (sleepcount % 10 == 0) {
printf("."); //printf(".");
fflush(stdout); //fflush(stdout);
} }
} }
printf("\n"); //printf("\n");
PX4_INFO("ARMING TEST: REARMING: STARTING RAMP"); //PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time(); starttime = hrt_absolute_time();
sleepcount = 0; sleepcount = 0;
@@ -366,7 +365,7 @@ int test_mixer(int argc, char *argv[])
/* check ramp */ /* check ramp */
fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]); //fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
@@ -388,46 +387,42 @@ int test_mixer(int argc, char *argv[])
sleepcount++; sleepcount++;
if (sleepcount % 10 == 0) { if (sleepcount % 10 == 0) {
printf("."); // printf(".");
fflush(stdout); // fflush(stdout);
} }
} }
printf("\n"); //printf("\n");
/* load multirotor at once test */ /* load multirotor at once test */
mixer_group.reset(); mixer_group.reset();
if (argc > 2) { #if !defined(CONFIG_ARCH_BOARD_SITL)
filename = argv[2]; filename = "/etc/mixers/quad_test.mix";
#else
} else { filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
filename = "/etc/mixers/quad_test.mix"; #endif
}
load_mixer_file(filename, &buf[0], sizeof(buf)); load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf); loaded = strlen(buf);
fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); //fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
unsigned mc_loaded = loaded; unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded); mixer_group.load_from_buf(&buf[0], mc_loaded);
PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); //PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 5) { if (mixer_group.count() != 5) {
PX4_ERR("FAIL: Quad test mixer load failed"); PX4_ERR("FAIL: Quad test mixer load failed");
return 1; return 1;
} }
PX4_INFO("SUCCESS: No errors in mixer test"); //PX4_INFO("SUCCESS: No errors in mixer test");
return 0; return 0;
} }
static int static int
mixer_callback(uintptr_t handle, mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
uint8_t control_group,
uint8_t control_index,
float &control)
{ {
if (control_group != 0) { if (control_group != 0) {
return -1; return -1;

49
unittests/uorb_unittests/uORBGtestTopics.hpp → src/systemcmds/tests/test_perf.c
View File

@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2015 Mark Charlebois. All rights reserved. * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -31,28 +31,39 @@
* *
****************************************************************************/ ****************************************************************************/
#ifndef _uORBGtestTopics_hpp_ #include <px4_config.h>
#define _uORBGtestTopics_hpp_ #include <px4_posix.h>
#include "uORB/uORB.h" #include <systemlib/perf_counter.h>
namespace uORB_test #include "tests.h"
int
test_perf(int argc, char *argv[])
{ {
struct orb_topic_A perf_counter_t cc = perf_alloc(PC_COUNT, "test_count");
{ perf_counter_t ec = perf_alloc(PC_ELAPSED, "test_elapsed");
int16_t val;
};
struct orb_topic_B if ((cc == NULL) || (ec == NULL)) {
{ printf("perf: counter alloc failed\n");
int16_t val; return 1;
}; }
ORB_DEFINE(topicA, struct orb_topic_A, nullptr, "TOPICA:int16 val;"); perf_begin(ec);
ORB_DEFINE(topicB, struct orb_topic_B, nullptr, "TOPICB:int16 val;"); perf_count(cc);
perf_count(cc);
perf_count(cc);
perf_count(cc);
printf("perf: expect count of 4\n");
perf_print_counter(cc);
perf_end(ec);
printf("perf: expect count of 1\n");
perf_print_counter(ec);
printf("perf: expect at least two counters\n");
perf_print_all(0);
ORB_DEFINE(topicA_clone, struct orb_topic_A, nullptr, "TOPICA_CLONE:int16 val;"; perf_free(cc);
ORB_DEFINE(topicB_clone, struct orb_topic_B, nullptr, "TOPICB_CLONE:int16 val;"); perf_free(ec);
return OK;
} }
#endif // _UnitTest_uORBTopics_hpp_

34
src/systemcmds/tests/test_uart_baudchange.c
View File

@@ -57,40 +57,6 @@
#include <math.h> #include <math.h>
#include <float.h> #include <float.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
int test_uart_baudchange(int argc, char *argv[]) int test_uart_baudchange(int argc, char *argv[])
{ {
int uart2_nwrite = 0; int uart2_nwrite = 0;

34
src/systemcmds/tests/test_uart_console.c
View File

@@ -56,40 +56,6 @@
#include <float.h> #include <float.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
static void *receive_loop(void *arg) static void *receive_loop(void *arg)
{ {
int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY); int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);

72
src/systemcmds/tests/tests.h
View File

@@ -70,50 +70,48 @@
# endif # endif
#endif #endif
/****************************************************************************
* Public Types
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
__BEGIN_DECLS __BEGIN_DECLS
extern int test_sensors(int argc, char *argv[]);
extern int test_gpio(int argc, char *argv[]);
extern int test_hrt(int argc, char *argv[]);
extern int test_tone(int argc, char *argv[]);
extern int test_led(int argc, char *argv[]);
extern int test_adc(int argc, char *argv[]); extern int test_adc(int argc, char *argv[]);
extern int test_int(int argc, char *argv[]); extern int test_autodeclination(int argc, char *argv[]);
extern int test_float(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_cpuload(int argc, char *argv[]);
extern int test_uart_send(int argc, char *argv[]);
extern int test_sleep(int argc, char *argv[]);
extern int test_time(int argc, char *argv[]);
extern int test_uart_console(int argc, char *argv[]);
extern int test_hott_telemetry(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
extern int test_bson(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]); extern int test_file(int argc, char *argv[]);
extern int test_file2(int argc, char *argv[]); extern int test_file2(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]); extern int test_float(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]); extern int test_hott_telemetry(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]);
extern int test_int(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_led(int argc, char *argv[]);
extern int test_mathlib(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]);
extern int test_eigen(int argc, char *argv[]); extern int test_matrix(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
extern int test_perf(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
extern int test_sensors(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_sleep(int argc, char *argv[]);
extern int test_time(int argc, char *argv[]);
extern int test_tone(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_uart_console(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_send(int argc, char *argv[]);
/* external */
extern int commander_tests_main(int argc, char *argv[]);
extern int mavlink_tests_main(int argc, char *argv[]);
extern int controllib_test_main(int argc, char *argv[]);
extern int uorb_tests_main(int argc, char *argv[]);
extern int rc_tests_main(int argc, char *argv[]);
extern int sf0x_tests_main(int argc, char *argv[]);
__END_DECLS __END_DECLS

244
src/systemcmds/tests/tests_main.c
View File

@@ -38,20 +38,17 @@
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
*/ */
#include "tests.h"
#include <px4_config.h> #include <px4_config.h>
#include <sys/types.h> #include <sys/types.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h>
#include <string.h> #include <string.h>
#include <unistd.h>
#include <fcntl.h> #include <fcntl.h>
#include <errno.h> #include <errno.h>
#include <arch/board/board.h>
#include <systemlib/perf_counter.h>
// Not using Eigen at the moment // Not using Eigen at the moment
#define TESTS_EIGEN_DISABLE #define TESTS_EIGEN_DISABLE
@@ -62,8 +59,9 @@
****************************************************************************/ ****************************************************************************/
static int test_help(int argc, char *argv[]); static int test_help(int argc, char *argv[]);
static int test_runner(unsigned option);
static int test_all(int argc, char *argv[]); static int test_all(int argc, char *argv[]);
static int test_perf(int argc, char *argv[]);
static int test_jig(int argc, char *argv[]); static int test_jig(int argc, char *argv[]);
/**************************************************************************** /****************************************************************************
@@ -78,45 +76,52 @@ const struct {
#define OPT_NOALLTEST (1<<1) #define OPT_NOALLTEST (1<<1)
#define OPT_NOJIGTEST (1<<2) #define OPT_NOJIGTEST (1<<2)
} tests[] = { } tests[] = {
#ifdef __PX4_NUTTX {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{"led", test_led, 0},
{"time", test_time, OPT_NOJIGTEST},
{"sensors", test_sensors, 0},
{"adc", test_adc, OPT_NOJIGTEST},
#endif /* __PX4_NUTTX */
{"int", test_int, 0},
{"float", test_float, 0},
{"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
{"tone", test_tone, 0},
{"sleep", test_sleep, OPT_NOJIGTEST},
{"perf", test_perf, OPT_NOJIGTEST},
{"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST}, {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST},
{"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST}, {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST},
{"param", test_param, 0}, #ifdef __PX4_NUTTX
{"adc", test_adc, OPT_NOJIGTEST},
{"led", test_led, 0},
{"sensors", test_sensors, 0},
{"time", test_time, OPT_NOJIGTEST},
{"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST},
#else
{"rc", rc_tests_main, 0},
#endif /* __PX4_NUTTX */
/* external tests */
{"commander", commander_tests_main, 0},
{"controllib", controllib_test_main, 0},
//{"mavlink", mavlink_tests_main, 0}, // TODO: fix mavlink_tests
{"sf0x", sf0x_tests_main, 0},
{"uorb", uorb_tests_main, 0},
{"autodeclination", test_autodeclination, 0},
{"bson", test_bson, 0}, {"bson", test_bson, 0},
{"conv", test_conv, 0},
{"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST}, {"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST},
{"file2", test_file2, OPT_NOJIGTEST}, {"file2", test_file2, OPT_NOJIGTEST},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"float", test_float, 0},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
#ifndef TESTS_MATHLIB_DISABLE {"int", test_int, 0},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"mathlib", test_mathlib, 0}, {"mathlib", test_mathlib, 0},
#endif {"matrix", test_matrix, 0},
#ifndef TESTS_EIGEN_DISABLE {"mixer", test_mixer, OPT_NOJIGTEST},
{"eigen", test_eigen, OPT_NOJIGTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
#endif {"param", test_param, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {"perf", test_perf, OPT_NOJIGTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"sleep", test_sleep, OPT_NOJIGTEST},
{"tone", test_tone, 0},
{"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST},
{NULL, NULL, 0} {NULL, NULL, 0}
}; };
@@ -138,18 +143,30 @@ test_help(int argc, char *argv[])
static int static int
test_all(int argc, char *argv[]) test_all(int argc, char *argv[])
{
return test_runner(OPT_NOALLTEST);
}
static int
test_jig(int argc, char *argv[])
{
return test_runner(OPT_NOJIGTEST);
}
static int
test_runner(unsigned option)
{ {
unsigned i; unsigned i;
char *args[2] = {"all", NULL}; char *args[2] = {"all", NULL};
unsigned int failcount = 0; unsigned int failcount = 0;
unsigned int testcount = 0; unsigned int testcount = 0;
bool passed[NTESTS]; unsigned passed[NTESTS];
printf("\nRunning all tests...\n\n"); printf("\nRunning all tests...\n\n");
for (i = 0; tests[i].name; i++) { for (i = 0; tests[i].name; i++) {
/* Only run tests that are not excluded */ /* Only run tests that are not excluded */
if (!(tests[i].options & OPT_NOALLTEST)) { if (!(tests[i].options & option)) {
for (int j = 0; j < 80; j++) { for (int j = 0; j < 80; j++) {
printf("-"); printf("-");
} }
@@ -164,12 +181,12 @@ test_all(int argc, char *argv[])
fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name); fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name);
fflush(stderr); fflush(stderr);
failcount++; failcount++;
passed[i] = false; passed[i] = 0;
} else { } else {
printf(" [%s] \t\tPASS\n", tests[i].name); printf(" [%s] \t\tPASS\n", tests[i].name);
fflush(stdout); fflush(stdout);
passed[i] = true; passed[i] = 1;
} }
for (int j = 0; j < 80; j++) { for (int j = 0; j < 80; j++) {
@@ -185,7 +202,7 @@ test_all(int argc, char *argv[])
/* Print summary */ /* Print summary */
printf("\n"); printf("\n");
for (int j = 0; j < 80; j++) { for (unsigned j = 0; j < 80; j++) {
printf("#"); printf("#");
} }
@@ -215,12 +232,12 @@ test_all(int argc, char *argv[])
printf("\n"); printf("\n");
/* Print failed tests */ /* Print failed tests */
if (failcount > 0) { printf(" Failed tests:\n\n"); } if (failcount > 0) {
printf(" Failed tests:\n\n");
}
unsigned int k; for (unsigned k = 0; k < i; k++) {
if (!passed[k] && !(tests[k].options & option)) {
for (k = 0; k < i; k++) {
if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
} }
} }
@@ -230,129 +247,6 @@ test_all(int argc, char *argv[])
return (failcount > 0); return (failcount > 0);
} }
static int
test_perf(int argc, char *argv[])
{
perf_counter_t cc, ec;
cc = perf_alloc(PC_COUNT, "test_count");
ec = perf_alloc(PC_ELAPSED, "test_elapsed");
if ((cc == NULL) || (ec == NULL)) {
printf("perf: counter alloc failed\n");
return 1;
}
perf_begin(ec);
perf_count(cc);
perf_count(cc);
perf_count(cc);
perf_count(cc);
printf("perf: expect count of 4\n");
perf_print_counter(cc);
perf_end(ec);
printf("perf: expect count of 1\n");
perf_print_counter(ec);
printf("perf: expect at least two counters\n");
perf_print_all(0);
perf_free(cc);
perf_free(ec);
return OK;
}
int test_jig(int argc, char *argv[])
{
unsigned i;
char *args[2] = {"jig", NULL};
unsigned int failcount = 0;
unsigned int testcount = 0;
bool passed[NTESTS];
printf("\nRunning all tests...\n\n");
for (i = 0; tests[i].name; i++) {
/* Only run tests that are not excluded */
if (!(tests[i].options & OPT_NOJIGTEST)) {
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n");
printf(" [%s] \t\tSTARTING TEST\n", tests[i].name);
fflush(stdout);
/* Execute test */
if (tests[i].fn(1, args) != 0) {
fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name);
fflush(stderr);
failcount++;
passed[i] = false;
} else {
printf(" [%s] \t\tPASS\n", tests[i].name);
fflush(stdout);
passed[i] = true;
}
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n");
testcount++;
}
}
/* Print summary */
printf("\n");
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n\n");
printf(" T E S T S U M M A R Y\n\n");
if (failcount == 0) {
printf(" ______ __ __ ______ __ __ \n");
printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n");
printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n");
printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n");
printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n");
printf("\n");
printf(" All tests passed (%d of %d)\n", testcount, testcount);
} else {
printf(" ______ ______ __ __ \n");
printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n");
printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n");
printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n");
printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n");
printf("\n");
printf(" Some tests failed (%d of %d)\n", failcount, testcount);
}
printf("\n");
/* Print failed tests */
if (failcount > 0) { printf(" Failed tests:\n\n"); }
for (int k = 0; k < i; k++) {
if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
}
}
fflush(stdout);
return 0;
}
__EXPORT int tests_main(int argc, char *argv[]); __EXPORT int tests_main(int argc, char *argv[]);
/** /**
@@ -360,19 +254,17 @@ __EXPORT int tests_main(int argc, char *argv[]);
*/ */
int tests_main(int argc, char *argv[]) int tests_main(int argc, char *argv[])
{ {
unsigned i;
if (argc < 2) { if (argc < 2) {
printf("tests: missing test name - 'tests help' for a list of tests\n"); printf("tests: missing test name - 'tests help' for a list of tests\n");
return 1; return 1;
} }
for (i = 0; tests[i].name; i++) { for (unsigned i = 0; tests[i].name; i++) {
if (!strcmp(tests[i].name, argv[1])) { if (!strcmp(tests[i].name, argv[1])) {
return tests[i].fn(argc - 1, argv + 1); return tests[i].fn(argc - 1, argv + 1);
} }
} }
printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
return ERROR; return 1;
} }

127
unittests/CMakeLists.txt
View File

@@ -1,9 +1,5 @@
cmake_minimum_required(VERSION 2.8) cmake_minimum_required(VERSION 2.8)
include(CMakeForceCompiler)
#CMAKE_FORCE_C_COMPILER(clang Clang)
#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
add_compile_options(-Qunused-arguments ) add_compile_options(-Qunused-arguments )
endif() endif()
@@ -14,28 +10,16 @@ endif()
project(unittests) project(unittests)
enable_testing() enable_testing()
include(CheckCXXCompilerFlag) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest) set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
add_subdirectory(${GTEST_DIR}) add_subdirectory(${GTEST_DIR})
include_directories(${GTEST_DIR}/include) include_directories(${GTEST_DIR}/include)
set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src) set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test) set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
include_directories(${CMAKE_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR})
include_directories(${PX4_SITL_BUILD}/src) include_directories(${PX4_SITL_BUILD}/src)
@@ -52,38 +36,27 @@ include_directories(${PX4_SRC}/modules/uORB)
include_directories(${PX4_SRC}/platforms) include_directories(${PX4_SRC}/platforms)
include_directories(${PX4_SRC}/platforms/posix/include) include_directories(${PX4_SRC}/platforms/posix/include)
include_directories(${PX4_SRC}/platforms/posix/px4_layer) include_directories(${PX4_SRC}/platforms/posix/px4_layer)
include_directories(${PX4_SRC}/platforms/posix/work_queue)
add_definitions(-D__CUSTOM_FILE_IO__)
add_definitions(-D__EXPORT=) add_definitions(-D__EXPORT=)
add_definitions(-D__PX4_POSIX) add_definitions(-D__PX4_POSIX)
add_definitions(-D__PX4_TESTS) add_definitions(-D__PX4_TESTS)
add_definitions(-D__PX4_UNIT_TESTS)
add_definitions(-D_UNIT_TEST=) add_definitions(-D_UNIT_TEST=)
add_definitions(-DERROR=-1) add_definitions(-DERROR=-1)
add_definitions(-Dmain_t=int) add_definitions(-Dmain_t=int)
add_definitions(-Dnoreturn_function=) add_definitions(-Dnoreturn_function=)
add_definitions(-DOK=0) add_definitions(-DOK=0)
# check if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
add_custom_target(check add_definitions(-D__PX4_DARWIN)
COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure else()
WORKING_DIR ${CMAKE_BINARY_DIR} add_definitions(-D__PX4_LINUX)
USES_TERMINAL) endif()
function(add_gtest)
foreach(test_name ${ARGN})
if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
target_link_libraries(${test_name} gtest_main pthread px4_platform)
add_definitions(-D__PX4_DARWIN)
else()
target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
add_definitions(-D__PX4_LINUX)
endif()
add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
add_dependencies(check ${test_name})
endforeach()
endfunction()
add_library(px4_platform add_library(px4_platform
${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c ${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c
${PX4_SRC}/drivers/device/device_posix.cpp ${PX4_SRC}/drivers/device/device_posix.cpp
${PX4_SRC}/drivers/device/i2c_posix.cpp ${PX4_SRC}/drivers/device/i2c_posix.cpp
${PX4_SRC}/drivers/device/ringbuffer.cpp ${PX4_SRC}/drivers/device/ringbuffer.cpp
@@ -112,8 +85,30 @@ add_library(px4_platform
${PX4_SRC}/platforms/posix/work_queue/work_lock.c ${PX4_SRC}/platforms/posix/work_queue/work_lock.c
${PX4_SRC}/platforms/posix/work_queue/work_queue.c ${PX4_SRC}/platforms/posix/work_queue/work_queue.c
${PX4_SRC}/platforms/posix/work_queue/work_thread.c ${PX4_SRC}/platforms/posix/work_queue/work_thread.c
) )
target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms) target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/include)
target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_queue)
# check
add_custom_target(check
COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
WORKING_DIR ${CMAKE_BINARY_DIR}
USES_TERMINAL)
# add_gtest
function(add_gtest)
foreach(test_name ${ARGN})
if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
target_link_libraries(${test_name} gtest_main pthread px4_platform)
else()
target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
endif()
add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
add_dependencies(check ${test_name})
endforeach()
endfunction()
####################################################################### #######################################################################
@@ -122,59 +117,9 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
# add_executable(example_test example_test.cpp) # add_executable(example_test example_test.cpp)
# add_gtest(example_test) # add_gtest(example_test)
# autodeclination_test
add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
add_gtest(autodeclination_test)
# mixer_test
add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
add_executable(mixer_test mixer_test.cpp
${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
add_gtest(mixer_test)
# conversion_test
add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
add_gtest(conversion_test)
# sbus2_test
add_executable(sbus2_test sbus2_test.cpp
${PX4_SRC}/lib/rc/sbus.c)
add_gtest(sbus2_test)
# DSM test
add_executable(dsm_test dsm_test.cpp
${PX4_SRC}/lib/rc/dsm.c)
add_gtest(dsm_test)
# st24_test
add_executable(rc_input_test st24_test.cpp sumd_test.cpp
${PX4_SRC}/lib/rc/st24.c
${PX4_SRC}/lib/rc/sumd.c)
add_gtest(rc_input_test)
# sf0x_test
add_executable(sf0x_test sf0x_test.cpp
${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
add_gtest(sf0x_test)
# param_test # param_test
add_executable(param_test param_test.cpp uorb_stub.cpp add_executable(param_test param_test.cpp uorb_stub.cpp
${PX4_SRC}/modules/systemlib/bson/tinybson.c ${PX4_SRC}/modules/systemlib/bson/tinybson.c
${PX4_SRC}/modules/systemlib/param/param.c) ${PX4_SRC}/modules/systemlib/param/param.c)
target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a) target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
add_gtest(param_test) add_gtest(param_test)
# param_shmem_test
#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
# ${PX4_SRC}/modules/systemlib/bson/tinybson.c
# ${PX4_SRC}/modules/systemlib/param/param_shmem.c)
#add_gtest(param_shmem_test)

18
unittests/autodeclination_test.cpp
View File

@@ -1,18 +0,0 @@
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <px4iofirmware/px4io.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include "gtest/gtest.h"
TEST(AutoDeclinationTest, AutoDeclination)
{
ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree";
}

10
unittests/conversion_test.cpp
View File

@@ -1,10 +0,0 @@
#include <systemlib/mixer/mixer.h>
#include <systemlib/err.h>
#include "../../src/systemcmds/tests/tests.h"
#include "gtest/gtest.h"
TEST(ConversionTest, quad_w_main)
{
ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed";
}

14
unittests/data/fit_linear_voltage.m
View File

@@ -1,14 +0,0 @@
close all;
clear all;
M = importdata('px4io_v1.3.csv');
voltage = M.data(:, 1);
counts = M.data(:, 2);
plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15);
coeffs = polyfit(counts, voltage, 1);
fittedC = linspace(min(counts), max(counts), 500);
fittedV = polyval(coeffs, fittedC);
hold on
plot(fittedC, fittedV, 'r-', 'LineWidth', 3);
slope = coeffs(1)
y_intersection = coeffs(2)

70
unittests/data/px4io_v1.3.csv
View File

@@ -1,70 +0,0 @@
voltage, counts
4.3, 950
4.4, 964
4.5, 986
4.6, 1009
4.7, 1032
4.8, 1055
4.9, 1078
5.0, 1101
5.2, 1124
5.3, 1148
5.4, 1171
5.5, 1195
6.0, 1304
6.1, 1329
6.2, 1352
7.0, 1517
7.1, 1540
7.2, 1564
7.3, 1585
7.4, 1610
7.5, 1636
8.0, 1728
8.1, 1752
8.2, 1755
8.3, 1798
8.4, 1821
9.0, 1963
9.1, 1987
9.3, 2010
9.4, 2033
10.0, 2174
10.1, 2198
10.2, 2221
10.3, 2245
10.4, 2268
11.0, 2385
11.1, 2409
11.2, 2432
11.3, 2456
11.4, 2480
11.5, 2502
11.6, 2526
11.7, 2550
11.8, 2573
11.9, 2597
12.0, 2621
12.1, 2644
12.3, 2668
12.4, 2692
12.5, 2716
12.6, 2737
12.7, 2761
13.0, 2832
13.5, 2950
13.6, 2973
14.1, 3068
14.2, 3091
14.7, 3209
15.0, 3280
15.1, 3304
15.5, 3374
15.6, 3397
15.7, 3420
16.0, 3492
16.1, 3514
16.2, 3538
16.9, 3680
17.0, 3704
17.1, 3728
1 voltage counts
2 4.3 950
3 4.4 964
4 4.5 986
5 4.6 1009
6 4.7 1032
7 4.8 1055
8 4.9 1078
9 5.0 1101
10 5.2 1124
11 5.3 1148
12 5.4 1171
13 5.5 1195
14 6.0 1304
15 6.1 1329
16 6.2 1352
17 7.0 1517
18 7.1 1540
19 7.2 1564
20 7.3 1585
21 7.4 1610
22 7.5 1636
23 8.0 1728
24 8.1 1752
25 8.2 1755
26 8.3 1798
27 8.4 1821
28 9.0 1963
29 9.1 1987
30 9.3 2010
31 9.4 2033
32 10.0 2174
33 10.1 2198
34 10.2 2221
35 10.3 2245
36 10.4 2268
37 11.0 2385
38 11.1 2409
39 11.2 2432
40 11.3 2456
41 11.4 2480
42 11.5 2502
43 11.6 2526
44 11.7 2550
45 11.8 2573
46 11.9 2597
47 12.0 2621
48 12.1 2644
49 12.3 2668
50 12.4 2692
51 12.5 2716
52 12.6 2737
53 12.7 2761
54 13.0 2832
55 13.5 2950
56 13.6 2973
57 14.1 3068
58 14.2 3091
59 14.7 3209
60 15.0 3280
61 15.1 3304
62 15.5 3374
63 15.6 3397
64 15.7 3420
65 16.0 3492
66 16.1 3514
67 16.2 3538
68 16.9 3680
69 17.0 3704
70 17.1 3728

74
unittests/dsm_test.cpp
View File

@@ -1,74 +0,0 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "../../src/systemcmds/tests/tests.h"
#include <drivers/drv_hrt.h>
// Enable DSM parser output
#define DSM_DEBUG
#include <rc/dsm.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include "gtest/gtest.h"
TEST(DSMTest, DSM)
{
const char *filepath = "testdata/dsm_x_data.txt";
FILE *fp;
fp = fopen(filepath, "rt");
ASSERT_TRUE(fp);
warnx("loading data from: %s", filepath);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
uint8_t frame[20];
uint16_t rc_values[18];
uint16_t num_values;
bool dsm_11_bit;
unsigned dsm_frame_drops = 0;
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
int rate_limiter = 0;
unsigned last_drop = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ASSERT_GT(ret, 0);
frame[0] = x;
unsigned len = 1;
// Pipe the data into the parser
bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values,
&dsm_11_bit, &dsm_frame_drops, max_channels);
if (result) {
warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
for (unsigned i = 0; i < num_values; i++) {
printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
}
}
if (last_drop != (dsm_frame_drops)) {
warnx("frame dropped, now #%d", (dsm_frame_drops));
last_drop = dsm_frame_drops;
}
rate_limiter++;
}
ASSERT_EQ(ret, EOF);
}

12
unittests/mixer_test.cpp
View File

@@ -1,12 +0,0 @@
#include <systemlib/mixer/mixer.h>
#include <systemlib/err.h>
#include "../../src/systemcmds/tests/tests.h"
#include "gtest/gtest.h"
TEST(MixerTest, Mixer)
{
const char *args[] = {"empty", "../ROMFS/px4fmu_test/mixers/IO_pass.mix", "../ROMFS/px4fmu_test/mixers/quad_test.mix"};
ASSERT_EQ(test_mixer(3, (char **)args), 0) << "IO_pass.mix failed";
}

89
unittests/sbus2_test.cpp
View File

@@ -1,89 +0,0 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "../../src/systemcmds/tests/tests.h"
#include <drivers/drv_hrt.h>
// Enable S.BUS parser output
#define SBUS_DEBUG
#include <rc/sbus.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include "gtest/gtest.h"
TEST(SBUS2Test, SBUS2)
{
const char *filepath = "testdata/sbus2_r7008SB.txt";
FILE *fp;
fp = fopen(filepath, "rt");
ASSERT_TRUE(fp);
warnx("loading data from: %s", filepath);
// if (argc < 2)
// errx(1, "Need a filename for the input file");
int byte_offset = 7;
// if (argc > 2) {
// char* end;
// byte_offset = strtol(argv[2],&end,10);
// }
warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
uint8_t frame[SBUS_BUFFER_SIZE];
uint16_t rc_values[18];
uint16_t num_values;
unsigned sbus_frame_drops = 0;
unsigned sbus_frame_resets = 0;
bool sbus_failsafe;
bool sbus_frame_drop;
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
int rate_limiter = 0;
unsigned last_drop = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ASSERT_GT(ret, 0);
frame[0] = x;
unsigned len = 1;
// Pipe the data into the parser
hrt_abstime now = hrt_absolute_time();
// if (rate_limiter % byte_offset == 0) {
bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
&sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
if (result) {
//warnx("decoded packet");
}
// }
if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
last_drop = sbus_frame_drops + sbus_frame_resets;
}
rate_limiter++;
}
ASSERT_EQ(ret, EOF);
}

63
unittests/st24_test.cpp
View File

@@ -1,63 +0,0 @@
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <rc/st24.h>
#include "../../src/systemcmds/tests/tests.h"
#include "gtest/gtest.h"
TEST(ST24Test, ST24)
{
const char *filepath = "testdata/st24_data.txt";
//warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
ASSERT_TRUE(fp);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[20];
if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ASSERT_EQ(EOF, ret);
}

63
unittests/sumd_test.cpp
View File

@@ -1,63 +0,0 @@
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <rc/sumd.h>
#include "../../src/systemcmds/tests/tests.h"
#include "gtest/gtest.h"
TEST(SUMDTest, SUMD)
{
const char *filepath = "testdata/sumd_data.txt";
//warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
//ASSERT_TRUE(fp);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[32];
if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ASSERT_EQ(EOF, ret);
}

2
unittests/uorb_stub.cpp
View File

@@ -19,5 +19,3 @@ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void
{ {
return 0; return 0;
} }

211
unittests/uorb_unittests/uORBCommunicatorMock.cpp
View File

@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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 "uORBCommunicatorMock.hpp"
#include "uORB/uORB.h"
#include "uORBGtestTopics.hpp"
#include "px4_log.h"
#include <string.h>
#include "uORBManager.hpp"
#define LOG_TAG "uORBCommunicatorMock.cpp"
uORB_test::uORBCommunicatorMock::uORBCommunicatorMock()
: _rx_handler( nullptr )
{
/*
_sub_topicA_copy_fd = orb_subscribe( ORB_ID( topicA_copy ), (void*)&_sub_semaphore );
_sub_topicB_copy_fd = orb_subscribe( ORB_ID( topicB_copy), nullptr );
*/
_topic_translation_map[ "topicA" ] = "topicA_copy";
_topic_translation_map[ "topicB" ] = "topicB_copy";
_topic_translation_map[ "topicA_copy" ] = "topicA";
_topic_translation_map[ "topicB_copy" ] = "topicB";
}
int16_t uORB_test::uORBCommunicatorMock::add_subscription
(
const char * messageName,
int32_t msgRateInHz
)
{
int16_t rc = 0;
PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz );
_msgCounters[messageName]._add_subscriptionCount++;
/*
int16_t rc = -1;
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
rc = _rx_handler->process_add_subscription
(
_topic_translation_map[messageName],
msgRateInHz
);
}
}
*/
return rc;
}
int16_t uORB_test::uORBCommunicatorMock::remove_subscription
(
const char * messageName
)
{
int16_t rc = 0;
PX4_INFO( "got remove_subscription for msg[%s]", messageName );
_msgCounters[messageName]._remove_subscriptionCount++;
/*
int16_t rc = -1;
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
rc = _rx_handler->process_remove_subscription
(
_topic_translation_map[messageName]
);
}
}
*/
return rc;
}
int16_t uORB_test::uORBCommunicatorMock::register_handler
(
uORBCommunicator::IChannelRxHandler* handler
)
{
int16_t rc = 0;
_rx_handler = handler;
return rc;
}
int16_t uORB_test::uORBCommunicatorMock::send_message
(
const char * messageName,
int32_t length,
uint8_t* data
)
{
int16_t rc = 0;
PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length );
if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) )
{
_msgCounters[messageName]._send_messageCount++;
if( strcmp(messageName, "topicA") == 0 )
{
memcpy( &_topicAData, (void*)data, length );
}
else if( strcmp(messageName, "topicB") == 0 )
{
memcpy( &_topicBData, (void*)data, length );
}
else
{
//EPRINTF( "error messageName[%s] is not supported", messageName );
}
}
/*
int16_t rc = -1;
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
rc = _rx_handler->process_received_message
( _topic_translation_map[messageName], length, data );
}
}
*/
return rc;
}
bool uORB_test::uORBCommunicatorMock::get_remote_topicA_data
(
struct orb_topic_A* data
)
{
bool rc = false;
memcpy( data, &_topicAData, sizeof(_topicAData) );
rc = true;
/*
if( orb_copy(ORB_ID(topicA_copy), _sub_topicA_copy_fd, data) == OK )
{
rc = true;
}
*/
return rc;
}
bool uORB_test::uORBCommunicatorMock::get_remote_topicB_data
(
struct orb_topic_B* data
)
{
bool rc = false;
memcpy( data, &_topicBData, sizeof(_topicBData) );
rc = true;
/*
if( orb_copy(ORB_ID(topicB_copy), _sub_topicB_copy_fd, data) == OK )
{
rc = true;
}
*/
return rc;
}
void uORB_test::uORBCommunicatorMock::reset_counters()
{
InterfaceCounters resetCounter;
resetCounter._add_subscriptionCount = 0;
resetCounter._remove_subscriptionCount = 0;
resetCounter._send_messageCount = 0;
std::map<std::string, InterfaceCounters>::iterator it;
for( it = _msgCounters.begin(); it != _msgCounters.end(); ++it )
{
it->second = resetCounter;
}
}
uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const char * messageName )
{
return _msgCounters[ messageName ];
}

140
unittests/uorb_unittests/uORBCommunicatorMock.hpp
View File

@@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#ifndef _uORBCommunicatorMock_hpp_
#define _uORBCommunicatorMock_hpp_
#include "uORB/uORBCommunicator.hpp"
#include "uORBGtestTopics.hpp"
#include <map>
#include <string>
#include <set>
namespace uORB_test
{
class uORBCommunicatorMock;
}
class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel
{
public:
//counters to track how many times the iterface is called from
// uorb.
typedef struct
{
int64_t _add_subscriptionCount;
int64_t _remove_subscriptionCount;
int64_t _send_messageCount;
}InterfaceCounters;
uORBCommunicatorMock();
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz );
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription( const char * messageName );
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler );
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data);
uORBCommunicator::IChannelRxHandler* get_rx_handler()
{
return _rx_handler;
}
bool get_remote_topicA_data( struct orb_topic_A* data );
bool get_remote_topicB_data( struct orb_topic_B* data );
void reset_counters();
InterfaceCounters get_interface_counters( const char * messageName );
private:
uORBCommunicator::IChannelRxHandler* _rx_handler;
//int _sub_topicA_copy_fd;
//int _sub_topicB_copy_fd;
std::map<std::string, std::string> _topic_translation_map;
struct orb_topic_A _topicAData;
struct orb_topic_B _topicBData;
std::map<std::string, InterfaceCounters> _msgCounters;
};
#endif /* _uORBCommunicatorMock_test_hpp_ */

138
unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
View File

@@ -1,138 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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 "uORBCommunicatorMockLoopback.hpp"
#include "uORB/uORB.h"
#include "uORBGtestTopics.hpp"
#include "uORBManager.hpp"
#include <string.h>
#include "px4_log.h"
#define LOG_TAG "uORBCommunicatorMockLoopback.cpp"
uORB_test::uORBCommunicatorMockLoopback::uORBCommunicatorMockLoopback()
: _rx_handler( nullptr )
{
//_sub_topicA_clone_fd = orb_subscribe( ORB_ID( topicA_clone ), (void*)&_sub_semaphore );
//_sub_topicB_clone_fd = orb_subscribe( ORB_ID( topicB_clone ), nullptr );
_topic_translation_map[ "topicA" ] = "topicA_clone";
_topic_translation_map[ "topicB" ] = "topicB_clone";
_topic_translation_map[ "topicA_clone" ] = "topicA";
_topic_translation_map[ "topicB_clone" ] = "topicB";
}
int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription
(
const char * messageName,
int32_t msgRateInHz
)
{
int16_t rc = -1;
PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz );
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
rc = _rx_handler->process_add_subscription
(
_topic_translation_map[messageName].c_str(),
msgRateInHz
);
}
}
return rc;
}
int16_t uORB_test::uORBCommunicatorMockLoopback::remove_subscription
(
const char * messageName
)
{
int16_t rc = -1;
PX4_INFO( "got remove_subscription for msg[%s]", messageName );
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
rc = _rx_handler->process_remove_subscription
(
_topic_translation_map[messageName].c_str()
);
}
}
return rc;
}
int16_t uORB_test::uORBCommunicatorMockLoopback::register_handler
(
uORBCommunicator::IChannelRxHandler* handler
)
{
int16_t rc = 0;
_rx_handler = handler;
return rc;
}
int16_t uORB_test::uORBCommunicatorMockLoopback::send_message
(
const char * messageName,
int32_t length,
uint8_t* data
)
{
int16_t rc = -1;
PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length );
if( _rx_handler )
{
if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
{
if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName].c_str() ) )
{
rc = _rx_handler->process_received_message
( _topic_translation_map[messageName].c_str(), length, data );
PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName, rc );
}
else
{
// this is eqvuilanet of not sending the message to the remote.
rc = 0;
}
}
}
return rc;
}

133
unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
View File

@@ -1,133 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#ifndef _uORBCommunicatorMockLoopback_hpp_
#define _uORBCommunicatorMockLoopback_hpp_
#include "uORB/uORBCommunicator.hpp"
#include "uORBGtestTopics.hpp"
#include <map>
#include <string>
#include <set>
namespace uORB_test
{
class uORBCommunicatorMockLoopback;
}
class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChannel
{
public:
uORBCommunicatorMockLoopback();
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription( const char * messageName, int32_t msgRateInHz );
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription( const char * messageName );
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler );
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data);
uORBCommunicator::IChannelRxHandler* get_rx_handler()
{
return _rx_handler;
}
/*
bool get_remote_topicA_data( struct orb_topic_A* data );
bool get_remote_topicB_data( struct orb_topic_B* data );
*/
private:
uORBCommunicator::IChannelRxHandler* _rx_handler;
/*
int _sub_topicA_clone_fd;
int _sub_topicB_clone_fd;
pal::Semaphore _sub_semaphore;
*/
std::map<std::string, std::string> _topic_translation_map;
/*
struct orb_topic_A _topicAData;
struct orb_topic_B _topicBData;
*/
};
#endif /* _uORBCommunicatorMockLoopback_hpp_ */

483
unittests/uorb_unittests/uORBCommunicator_gtests.cpp
View File

@@ -1,483 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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 "uORBCommunicatorMock.hpp"
#include "uORBCommunicatorMockLoopback.hpp"
#include "gtest/gtest.h"
#include "uORB.h"
#include "uORBManager.hpp"
#include "uORBGtestTopics.hpp"
#include "uORBDevices.hpp"
#include "px4_log.h"
#include <errno.h>
#define LOG_TAG "uORBCommunicator_gtests.cpp"
namespace px4
{
void init_once();
}
namespace uORB_test
{
class uORBCommunicatorTest : public ::testing::Test
{
public:
uORBCommunicatorTest() :
_masterDevice( nullptr )
{
px4::init_once();
// create the Master Device and initialize it
_masterDevice = new uORB::DeviceMaster(uORB::PUBSUB);
if( _masterDevice != nullptr )
{
_masterDevice->init();
}
// get the uORB::Manager and set the mock instance
// explicitly.
uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel );
}
protected:
uORB_test::uORBCommunicatorMock _comm_channel;
struct orb_topic_A _topicA;
struct orb_topic_B _topicB;
orb_advert_t _pub_ptr;
int _sub_fd;
uORB::DeviceMaster* _masterDevice;
};
//================= Unit tests for add_subscription ===================
// this test will validate if the uORB calls the "add_subscription"
// method if there is atleast one subscriber in the local system.
//=====================================================================
TEST_F( uORBCommunicatorTest, add_subscription_case1 )
{
// case 1: add subscription should not be called if there are no
// internal subscriber and only a publisher.
// Steps:
// 0. reset the interface counters.
// 1. advertize a topic
// 2. check to see if add_subscription is called.
// the count should be zero.
//step 0.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._add_subscriptionCount, 0 );
//step 1.
_topicA.val = 1;
ASSERT_NE( ( _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA) ), nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
//step 2.
c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._add_subscriptionCount, 0 );
}
TEST_F( uORBCommunicatorTest, add_subscription_case2 )
{
// case 1: add subscription should not be called if there is atleast one
// internal subscriber and only a publisher.
// Steps:
// 0. reset the interface counters.
// 1. advertize a topic
// 2. Add in internal subscriber.
// 3. check to see if add_subscription is called.
// the count should be 1.
// 4. unsubscribe the topic
//step 0.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._add_subscriptionCount, 0 );
//step 1.
_topicA.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA);
PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%p)", _pub_ptr );
ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
// step 2
ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) ) ) , -1 ) << "Subscribe failed: %d" << errno;
PX4_INFO( "subscribe fd: %d", _sub_fd );
//step 3.
c = _comm_channel.get_interface_counters( "topicA" );
PX4_INFO( "interface counter for topicA: %d", (int)c._add_subscriptionCount );
ASSERT_EQ( c._add_subscriptionCount, 1 );
//step 4.
PX4_INFO( "Before unsubscribe for Topic A" );
ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
PX4_INFO( "After unsubscribe for Topic A" );
}
//============ unit tests for remove_subscribtion =======
TEST_F( uORBCommunicatorTest, remove_subscribtion )
{
// remove subscription should be called if there after a subscription is removed.
// Steps:
// 0. reset the interface counters.
// 1. advertize a topic
// 2. Add in internal subscriber.
// 3. unsubscribe the topic
// 4. check the remove_subsciption counter it should be 1.
//step 0.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._remove_subscriptionCount, 0 );
//step 1.
// step 1.
_topicA.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA);
PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%p)", _pub_ptr );
ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._remove_subscriptionCount, 0 );
// step 2
ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) )), -1 ); // << "Subscribe failed: " << _sub_fd << "errono: " << errno;
PX4_INFO( "subscribe fd: %d", _sub_fd );
c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._remove_subscriptionCount, 0 );
//step 3.
ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
//step 4.
c = _comm_channel.get_interface_counters( "topicA" );
ASSERT_EQ( c._remove_subscriptionCount, 1 );
}
//============ unit tests for send_message =======
TEST_F(uORBCommunicatorTest, send_message_case1 )
{
// Case1: send message should not be called when a topic is advertized
// and there are no remote remote subscribers.
// Steps:
// 0. reset the interface counters.
// 1. advertize a topic
// 2. check to see that the send message counter is 0.
//step 0.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA_sndmsg" );
ASSERT_EQ( c._send_messageCount, 0 );
//step 1.
ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, nullptr, "TOPICA_SNDMSG:int16_t val;" );
_topicA.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA );
ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
c = _comm_channel.get_interface_counters( "topicA_sndmsg" );
ASSERT_EQ( c._send_messageCount, 0 );
}
TEST_F(uORBCommunicatorTest, send_message_case2 )
{
// Case2: send message should be called when a topic is advertized
// and there are remote remote subscribers.
// Steps:
// 0. reset the interface counters.
// 1. Add a remote subscriber by calling the "process_add_subscription"
// 2. advertize a topic
// 3. check to see that the send message counter is 1 and check the value.
// 4. publish new data.
// 5. check to see that send_msg is incremented by 1 and check the value.
// 6. remove remote subscriber by calling the "process_remove_subscription"
// 7. publish new data.
// 8. check to see the sndmessage counter, it should not increment.
//step 0.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 0 );
//step 1.
uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
// add a local subsciber to avoid the issue of creating a node for the first time
// remote.
ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB) ) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
PX4_INFO( "subscribe fd: %d", _sub_fd );
ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 );
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 0 );
// step 2.
_topicB.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
//step 3.
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 1 );
struct orb_topic_B test_val;
_comm_channel.get_remote_topicB_data( &test_val );
ASSERT_EQ( test_val.val, 1 );
//step 4. publish new data.
_topicB.val = 2;
ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK );
//step 5.
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 2 );
_comm_channel.get_remote_topicB_data( &test_val );
ASSERT_EQ( test_val.val, 2 );
// step 6.
ASSERT_EQ( rx_handler->process_remove_subscription( "topicB" ), 0 );
ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
//step 7. publish new data.
_topicB.val = 5;
ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK );
//step 8.
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 2 );
_comm_channel.get_remote_topicB_data( &test_val );
ASSERT_EQ( test_val.val, 2 );
}
//========== UNIT tests to verify the process_receive_message interface
//========== of rx handler.
TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case1 )
{
// this will mimic the process of calling the process_receive_message
// from remote and verify that the local subscribers got it
// are the steps.
// 1. clear the counters.
// 2. advertize a topic
// 3. add a local subscriber.
// 4. call process_receive_message.
// 5. check to see that the subscriber got the data and the message is not sent back
// by looking at the counter for snd message.
//step 1.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 0 );
// step 2.
_topicB.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
// step 3.
ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
PX4_INFO( "subscribe fd: %d", _sub_fd );
//step 4.
uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
struct orb_topic_B testVal;
testVal.val = 10;
ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 );
// step 5. check the values.
struct orb_topic_B receivedVal;
ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( testVal.val, receivedVal.val )
<< "copy(1) mismatch: " << testVal.val
<< " expected " << receivedVal.val;
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 0 );
// cleanup.
ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
}
TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case2 )
{
// this will mimic the process of calling the process_receive_message
// from remote and verify that the local subscribers got it
// and also the message is send back to the remote. The following
// are the steps.
// 1. clear the counters.
// 2. advertize a topic
// 3. add a local subscriber.
// 4. add remote subscriber by calling the "process_add_subscription.
// 5. call process_receive_message.
// 6. check to see that the subscriber got the data and the message is not sent back
// by looking at the counter for snd message.
//step 1.
_comm_channel.reset_counters();
uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 0 );
// step 2.
_topicB.val = 1;
_pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
// step 3.
ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
PX4_INFO( "subscribe fd: %d", _sub_fd );
//step 4.
uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 );
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 1 );
//step 5.
struct orb_topic_B testVal;
testVal.val = 15;
ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 );
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 1 );
// step 6. check the values.
struct orb_topic_B receivedVal;
ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( testVal.val, receivedVal.val )
<< "copy(1) mismatch: " << testVal.val
<< " expected " << receivedVal.val;
c = _comm_channel.get_interface_counters( "topicB" );
ASSERT_EQ( c._send_messageCount, 1 );
// cleanup.
ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
}
TEST_F( uORBCommunicatorTest, Loopback )
{
// create the loopback instance.
uORB_test::uORBCommunicatorMockLoopback _comm_channel_loopback;
//intiailize the uorb to remove the node map entries;
//orb_initialize();
//set the communucation channel interface.
uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel_loopback );
// now for the actual test.
orb_advert_t pub_topicA_ptr;
int sub_topicA_fd;
int sub_topicAClone_fd;
struct orb_topic_A topicA;
struct orb_topic_A topicAClone;
struct orb_topic_A topicALocal;
// step 1.
topicA.val = 10;
pub_topicA_ptr = orb_advertise(ORB_ID(topicA), &topicA);
PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%p)", pub_topicA_ptr );
ASSERT_TRUE( pub_topicA_ptr != nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
PX4_INFO( "publist handle: 0x%08lx", (unsigned long)pub_topicA_ptr );
// step 2.
ASSERT_NE( ( sub_topicA_fd = orb_subscribe(ORB_ID(topicA)) ), -1 ) << "Subscribe failed: %d" << errno;
PX4_INFO( "subscribe fd: %d", sub_topicA_fd );
// step 3. check to see that the subscriber got a value of 10.
ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( topicA.val, topicALocal.val )
<< "copy(1) mismatch: " << topicA.val
<< " expected " << topicALocal.val;
// subscribe a remote subscriber.
ASSERT_NE( ( sub_topicAClone_fd = orb_subscribe(ORB_ID(topicA_clone)) ), -1 ) << "Subscribe failed: %d" << errno;
PX4_INFO( "subscribe fd: %d", sub_topicAClone_fd );
ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( topicA.val, topicAClone.val )
<< "copy(1) mismatch: " << topicA.val
<< " expected " << topicAClone.val;
// publish a new data and check to see that the data is received on the remote.
topicA.val = 15;
ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
// check to see that the subscriber got a new value.
ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( topicA.val, topicALocal.val )
<< "copy(1) mismatch: " << topicA.val
<< " expected " << topicALocal.val;
ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( topicA.val, topicAClone.val )
<< "copy(1) mismatch: " << topicA.val
<< " expected " << topicAClone.val;
// remove the remote subscriber and make sure that the data is not received,
ASSERT_EQ( orb_unsubscribe( sub_topicAClone_fd ), OK );
// publish a new data and check to see that the data is received on local this should not crash.
topicA.val = 20;
ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
// check to see that the subscriber got a new value.
ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
ASSERT_EQ( topicA.val, topicALocal.val )
<< "copy(1) mismatch: " << topicA.val
<< " expected " << topicALocal.val;
//remove the local subscriber as well and publish new data; system should not crash.
ASSERT_EQ( orb_unsubscribe( sub_topicA_fd ), OK );
// publish a new data; this should not crash.
topicA.val = 25;
ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
}
}