From 6ab9dc0acf89a09fa91a83622fbb1beb3e00d410 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 29 Jul 2016 13:27:58 +0200
Subject: [PATCH] Testing cleanup from Daniel Agar
---
CMakeLists.txt | 2 +-
Makefile | 24 +-
ROMFS/px4fmu_test/init.d/rcS | 69 +-
cmake/configs/nuttx_px4fmu-v2_default.cmake | 28 +-
cmake/configs/nuttx_px4fmu-v2_test.cmake | 23 +-
cmake/configs/nuttx_px4fmu-v4_default.cmake | 13 +-
cmake/configs/posix_sitl_default.cmake | 5 +-
cmake/configs/posix_sitl_test.cmake | 110 ---
posix-configs/SITL/init/rcS_test_gazebo_iris | 25 -
posix-configs/SITL/init/rcS_test_jmavsim_iris | 25 -
posix-configs/SITL/init/rcS_tests | 12 +
src/drivers/sf0x/sf0x_tests/CMakeLists.txt | 43 ++
.../drivers/sf0x/sf0x_tests/SF0XTest.cpp | 53 +-
src/lib/rc/CMakeLists.txt | 1 +
src/lib/rc/rc_tests/CMakeLists.txt | 43 ++
src/lib/rc/rc_tests/RCTest.cpp | 306 ++++++++
.../lib/rc/rc_tests/test_data}/dsm_x_data.txt | 0
.../rc/rc_tests/test_data}/sbus2_r7008SB.txt | 0
.../lib/rc/rc_tests/test_data}/st24_data.txt | 0
.../lib/rc/rc_tests/test_data}/sumd_data.txt | 0
src/modules/mavlink/mavlink_ftp.h | 2 +-
.../mavlink_tests/mavlink_ftp_test.cpp | 4 +-
.../uORB/uORB_tests/uORBTest_UnitTest.cpp | 2 +-
src/modules/unit_test/unit_test.h | 14 +
src/systemcmds/tests/CMakeLists.txt | 33 +-
src/systemcmds/tests/test_autodeclination.cpp | 38 +
src/systemcmds/tests/test_conv.cpp | 4 +-
src/systemcmds/tests/test_eigen.cpp | 564 ---------------
src/systemcmds/tests/test_float.c | 287 --------
src/systemcmds/tests/test_float.cpp | 149 ++++
src/systemcmds/tests/test_int.c | 152 ----
src/systemcmds/tests/test_int.cpp | 61 ++
src/systemcmds/tests/test_mathlib.cpp | 352 +++++----
src/systemcmds/tests/test_matrix.cpp | 679 ++++++++++++++++++
src/systemcmds/tests/test_mixer.cpp | 87 ++-
.../systemcmds/tests/test_perf.c | 49 +-
src/systemcmds/tests/test_uart_baudchange.c | 34 -
src/systemcmds/tests/test_uart_console.c | 34 -
src/systemcmds/tests/tests.h | 66 +-
src/systemcmds/tests/tests_main.c | 228 ++----
unittests/CMakeLists.txt | 127 +---
unittests/autodeclination_test.cpp | 18 -
unittests/conversion_test.cpp | 10 -
unittests/data/fit_linear_voltage.m | 14 -
unittests/data/px4io_v1.3.csv | 70 --
unittests/dsm_test.cpp | 74 --
unittests/mixer_test.cpp | 12 -
unittests/sbus2_test.cpp | 89 ---
unittests/st24_test.cpp | 63 --
unittests/sumd_test.cpp | 63 --
unittests/uorb_stub.cpp | 2 -
.../uorb_unittests/uORBCommunicatorMock.cpp | 211 ------
.../uorb_unittests/uORBCommunicatorMock.hpp | 140 ----
.../uORBCommunicatorMockLoopback.cpp | 138 ----
.../uORBCommunicatorMockLoopback.hpp | 133 ----
.../uORBCommunicator_gtests.cpp | 483 -------------
56 files changed, 1891 insertions(+), 3377 deletions(-)
delete mode 100644 cmake/configs/posix_sitl_test.cmake
delete mode 100644 posix-configs/SITL/init/rcS_test_gazebo_iris
delete mode 100644 posix-configs/SITL/init/rcS_test_jmavsim_iris
create mode 100644 posix-configs/SITL/init/rcS_tests
create mode 100644 src/drivers/sf0x/sf0x_tests/CMakeLists.txt
rename unittests/sf0x_test.cpp => src/drivers/sf0x/sf0x_tests/SF0XTest.cpp (57%)
create mode 100644 src/lib/rc/rc_tests/CMakeLists.txt
create mode 100644 src/lib/rc/rc_tests/RCTest.cpp
rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/dsm_x_data.txt (100%)
rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/sbus2_r7008SB.txt (100%)
rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/st24_data.txt (100%)
rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/sumd_data.txt (100%)
create mode 100644 src/systemcmds/tests/test_autodeclination.cpp
delete mode 100644 src/systemcmds/tests/test_eigen.cpp
delete mode 100644 src/systemcmds/tests/test_float.c
create mode 100644 src/systemcmds/tests/test_float.cpp
delete mode 100644 src/systemcmds/tests/test_int.c
create mode 100644 src/systemcmds/tests/test_int.cpp
create mode 100644 src/systemcmds/tests/test_matrix.cpp
rename unittests/uorb_unittests/uORBGtestTopics.hpp => src/systemcmds/tests/test_perf.c (68%)
delete mode 100644 unittests/autodeclination_test.cpp
delete mode 100644 unittests/conversion_test.cpp
delete mode 100644 unittests/data/fit_linear_voltage.m
delete mode 100644 unittests/data/px4io_v1.3.csv
delete mode 100644 unittests/dsm_test.cpp
delete mode 100644 unittests/mixer_test.cpp
delete mode 100644 unittests/sbus2_test.cpp
delete mode 100644 unittests/st24_test.cpp
delete mode 100644 unittests/sumd_test.cpp
delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.cpp
delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.hpp
delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
delete mode 100644 unittests/uorb_unittests/uORBCommunicator_gtests.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3b7efe008..99a6061681 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -246,7 +246,7 @@ endforeach()
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_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_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
diff --git a/Makefile b/Makefile
index f65ac29c1a..4447c73093 100644
--- a/Makefile
+++ b/Makefile
@@ -174,9 +174,6 @@ posix_sitl_default:
posix_sitl_lpe:
$(call cmake-build,$@)
-posix_sitl_test:
- $(call cmake-build,$@)
-
posix_sitl_replay:
$(call cmake-build,$@)
@@ -271,14 +268,14 @@ checks_uavcan: \
check_px4fmu-v4_default_and_uavcan
checks_sitls: \
- check_posix_sitl_default \
- check_posix_sitl_test \
+ check_posix_sitl_default
checks_last: \
- check_unittest \
+ check_tests \
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:
$(call colorecho,"Checking formatting with astyle")
@@ -300,15 +297,18 @@ ifeq ($(VECTORCONTROL),1)
@rm -rf ROMFS/px4fmu_common/uavcan
endif
-unittest: posix_sitl_test
+unittest: posix_sitl_default
$(call cmake-build-other,unittest, ../unittests)
@(cd build_unittest && ctest -j2 --output-on-failure)
-tests: posix_sitl_test unittest
-
-test_onboard_sitl:
- @HEADLESS=1 make posix_sitl_test gazebo_iris
+run_tests_posix: posix_sitl_default
+ @mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
+ @mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
+ @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
qgc_firmware: \
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index b9eadfa865..53f7524be0 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -111,87 +111,22 @@ fi
sh /etc/init.d/rc.sensors
-# Check for flow sensor
-if px4flow start
-then
-fi
-
-if ll40ls start
-then
-fi
+ver all
#
# Run unit tests at board boot, reporting failure as needed.
# 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
-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
echo
echo "All Unit Tests PASSED"
rgbled rgb 20 255 20
else
echo
- echo "Some Unit Tests FAILED:${unit_test_failure_list}"
+ echo "Some Unit Tests FAILED"
rgbled rgb 255 20 20
fi
-ver all
-
free
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 8907cf82dd..c38d726702 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -53,22 +53,34 @@ set(config_module_list
# System commands
#
systemcmds/bl_update
+ systemcmds/config
+ systemcmds/dumpfile
+ #systemcmds/esc_calib
systemcmds/mixer
+ #systemcmds/motor_ramp
+ systemcmds/mtd
+ systemcmds/nshterm
systemcmds/param
systemcmds/perf
systemcmds/pwm
- #systemcmds/esc_calib
systemcmds/reboot
- #systemcmds/topic_listener
+ #systemcmds/sd_bench
systemcmds/top
- systemcmds/config
- systemcmds/nshterm
- systemcmds/mtd
- systemcmds/dumpfile
+ #systemcmds/topic_listener
systemcmds/ver
- #systemcmds/sd_bench
+
+ #
+ # 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/motor_ramp
#
# General system control
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 80f8649e3d..b72c5a8190 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -53,32 +53,34 @@ set(config_module_list
# System commands
#
systemcmds/bl_update
+ systemcmds/config
+ systemcmds/dumpfile
+ #systemcmds/esc_calib
systemcmds/mixer
+ systemcmds/motor_ramp
+ systemcmds/mtd
+ systemcmds/nshterm
systemcmds/param
systemcmds/perf
systemcmds/pwm
- systemcmds/esc_calib
systemcmds/reboot
- #systemcmds/topic_listener
+ #systemcmds/sd_bench
systemcmds/top
- systemcmds/config
- systemcmds/nshterm
- systemcmds/mtd
- systemcmds/dumpfile
+ #systemcmds/topic_listener
systemcmds/ver
- #systemcmds/sd_bench
- systemcmds/tests
- systemcmds/motor_ramp
#
# 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
#
# General system control
@@ -95,9 +97,10 @@ set(config_module_list
# Estimation modules (EKF/ SO3 / other filters)
#
modules/attitude_estimator_q
- modules/ekf_att_pos_estimator
+ #modules/ekf_att_pos_estimator
modules/position_estimator_inav
modules/local_position_estimator
+ modules/ekf2
#
# Vehicle Control
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 545dfb76da..fe356e49d6 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -69,9 +69,20 @@ set(config_module_list
systemcmds/dumpfile
systemcmds/ver
systemcmds/sd_bench
- systemcmds/tests
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
#
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 2de3ecedf8..6fa6d9c0b7 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -70,6 +70,7 @@ set(config_module_list
lib/launchdetection
lib/mathlib
lib/mathlib/math/filter
+ lib/rc
lib/runway_takeoff
lib/tailsitter_recovery
lib/terrain_estimation
@@ -79,9 +80,11 @@ set(config_module_list
#
# Testing
#
+ drivers/sf0x/sf0x_tests
+ lib/rc/rc_tests
modules/commander/commander_tests
modules/controllib_test
- #modules/mavlink/mavlink_tests
+ #modules/mavlink/mavlink_tests #TODO: fix mavlink_tests
modules/unit_test
modules/uORB/uORB_tests
systemcmds/tests
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
deleted file mode 100644
index c45b2fc39e..0000000000
--- a/cmake/configs/posix_sitl_test.cmake
+++ /dev/null
@@ -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")
diff --git a/posix-configs/SITL/init/rcS_test_gazebo_iris b/posix-configs/SITL/init/rcS_test_gazebo_iris
deleted file mode 100644
index 8aeacf1eff..0000000000
--- a/posix-configs/SITL/init/rcS_test_gazebo_iris
+++ /dev/null
@@ -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
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
deleted file mode 100644
index 8aeacf1eff..0000000000
--- a/posix-configs/SITL/init/rcS_test_jmavsim_iris
+++ /dev/null
@@ -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
diff --git a/posix-configs/SITL/init/rcS_tests b/posix-configs/SITL/init/rcS_tests
new file mode 100644
index 0000000000..c41a301e2b
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_tests
@@ -0,0 +1,12 @@
+uorb start
+param load
+param set SYS_RESTART_TYPE 0
+dataman start
+
+ver all
+
+sleep 1
+
+tests all
+
+shutdown
diff --git a/src/drivers/sf0x/sf0x_tests/CMakeLists.txt b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt
new file mode 100644
index 0000000000..3ded382609
--- /dev/null
+++ b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt
@@ -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 :
diff --git a/unittests/sf0x_test.cpp b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
similarity index 57%
rename from unittests/sf0x_test.cpp
rename to src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
index 56e673a06d..af95214c64 100644
--- a/unittests/sf0x_test.cpp
+++ b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
@@ -1,19 +1,35 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
+#include <unit_test/unit_test.h>
-#include <drivers/drv_hrt.h>
#include <drivers/sf0x/sf0x_parser.h>
+
#include <systemlib/err.h>
-#include "gtest/gtest.h"
+#include <stdio.h>
+#include <unistd.h>
+
+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);
+}
-TEST(SF0XTest, SF0X)
+bool SF0XTest::sf0xTest(void)
{
const char _LINE_MAX = 20;
- char _linebuf[_LINE_MAX];
- _linebuf[0] = '\0';
+ //char _linebuf[_LINE_MAX];
+ //_linebuf[0] = '\0';
const char *lines[] = {"0.01\r\n",
"0.02\r\n",
@@ -39,7 +55,6 @@ TEST(SF0XTest, SF0X)
unsigned _parsebuf_index = 0;
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
-
//printf("\n%s", _linebuf);
int parse_ret;
@@ -48,6 +63,19 @@ TEST(SF0XTest, SF0X)
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
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" : "");
}
}
@@ -56,5 +84,8 @@ TEST(SF0XTest, SF0X)
}
- warnx("test finished");
+ return true;
}
+
+ut_declare_test_c(sf0x_tests_main, SF0XTest)
+
diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt
index 15bbd71bb9..8f9471124e 100644
--- a/src/lib/rc/CMakeLists.txt
+++ b/src/lib/rc/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
MODULE lib__rc
COMPILE_FLAGS
-Os
+ -Wno-unused-result
SRCS
st24.c
sumd.c
diff --git a/src/lib/rc/rc_tests/CMakeLists.txt b/src/lib/rc/rc_tests/CMakeLists.txt
new file mode 100644
index 0000000000..42a5a1b1ba
--- /dev/null
+++ b/src/lib/rc/rc_tests/CMakeLists.txt
@@ -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 :
diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp
new file mode 100644
index 0000000000..d7a834d5c8
--- /dev/null
+++ b/src/lib/rc/rc_tests/RCTest.cpp
@@ -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)
+
diff --git a/unittests/testdata/dsm_x_data.txt b/src/lib/rc/rc_tests/test_data/dsm_x_data.txt
similarity index 100%
rename from unittests/testdata/dsm_x_data.txt
rename to src/lib/rc/rc_tests/test_data/dsm_x_data.txt
diff --git a/unittests/testdata/sbus2_r7008SB.txt b/src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt
similarity index 100%
rename from unittests/testdata/sbus2_r7008SB.txt
rename to src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt
diff --git a/unittests/testdata/st24_data.txt b/src/lib/rc/rc_tests/test_data/st24_data.txt
similarity index 100%
rename from unittests/testdata/st24_data.txt
rename to src/lib/rc/rc_tests/test_data/st24_data.txt
diff --git a/unittests/testdata/sumd_data.txt b/src/lib/rc/rc_tests/test_data/sumd_data.txt
similarity index 100%
rename from unittests/testdata/sumd_data.txt
rename to src/lib/rc/rc_tests/test_data/sumd_data.txt
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 998c6b3cc6..017ac07d67 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -50,7 +50,7 @@ class MavlinkFtpTest;
class MavlinkFTP : public MavlinkStream
{
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();
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index e45b91252e..39af11f955 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -236,7 +236,7 @@ bool MavlinkFtpTest::_list_test(void)
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.
bool MavlinkFtpTest::_list_eof_test(void)
{
@@ -263,7 +263,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
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)
{
MavlinkFTP::PayloadHeader payload;
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index 7bcf9d7e4f..0ff04ed402 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -130,7 +130,7 @@ int uORBTest::UnitTest::pubsublatency_main(void)
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;
} else {
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index f512100bb6..aeddaac88e 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -37,6 +37,17 @@
#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.
class __EXPORT UnitTest
{
@@ -92,6 +103,9 @@ protected:
} \
} 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
/// since it will give you better error reporting of the actual values being compared.
#define ut_compare(message, v1, v2) \
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index bbd989058c..06e6d86225 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -33,14 +33,26 @@
set(srcs
test_adc.c
+ test_autodeclination.cpp
test_bson.c
- test_float.c
+ test_conv.cpp
+ test_file.c
+ test_file2.c
+ test_float.cpp
test_gpio.c
test_hott_telemetry.c
test_hrt.c
- test_int.c
+ test_int.cpp
test_jig_voltages.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_servo.c
test_sleep.c
@@ -48,16 +60,7 @@ set(srcs
test_uart_console.c
test_uart_loopback.c
test_uart_send.c
- test_mixer.cpp
- test_mathlib.cpp
- test_file.c
- test_file2.c
tests_main.c
- test_params.c
- test_ppm_loopback.c
- test_rc.c
- test_conv.cpp
- test_mount.c
)
if(${OS} STREQUAL "nuttx")
@@ -69,11 +72,15 @@ endif()
px4_add_module(
MODULE systemcmds__tests
MAIN tests
- STACK_MAIN 9000
- STACK_MAX 9000
+ STACK_MAIN 10000
+ STACK_MAX 10000
COMPILE_FLAGS
+ ${MODULE_CFLAGS}
-Wno-unused-result
-Wno-float-equal
+ -Wno-missing-declarations
+ -Wno-double-promotion
+ -Wno-unknown-warning-option
-Os
SRCS ${srcs}
DEPENDS
diff --git a/src/systemcmds/tests/test_autodeclination.cpp b/src/systemcmds/tests/test_autodeclination.cpp
new file mode 100644
index 0000000000..af66c517ca
--- /dev/null
+++ b/src/systemcmds/tests/test_autodeclination.cpp
@@ -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)
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index e451755959..c26455eda8 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -59,7 +59,7 @@
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) {
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;
}
diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp
deleted file mode 100644
index 2ede1ad66c..0000000000
--- a/src/systemcmds/tests/test_eigen.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
deleted file mode 100644
index 83b102b41e..0000000000
--- a/src/systemcmds/tests/test_float.c
+++ /dev/null
@@ -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;
-}
diff --git a/src/systemcmds/tests/test_float.cpp b/src/systemcmds/tests/test_float.cpp
new file mode 100644
index 0000000000..9174c1dd4e
--- /dev/null
+++ b/src/systemcmds/tests/test_float.cpp
@@ -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)
diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c
deleted file mode 100644
index 7db4b48f9b..0000000000
--- a/src/systemcmds/tests/test_int.c
+++ /dev/null
@@ -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;
-}
diff --git a/src/systemcmds/tests/test_int.cpp b/src/systemcmds/tests/test_int.cpp
new file mode 100644
index 0000000000..65944e1731
--- /dev/null
+++ b/src/systemcmds/tests/test_int.cpp
@@ -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)
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 0883f42e8a..9de1f06686 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -31,11 +31,18 @@
*
****************************************************************************/
-/**
- * @file test_mathlib.cpp
- *
- * Mathlib test
- */
+#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>
#include <px4_log.h>
#include <stdio.h>
@@ -47,17 +54,33 @@
#include <systemlib/err.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"
#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;
-int test_mathlib(int argc, char *argv[])
+bool MathlibTest::testVector2(void)
{
- int rc = 0;
- PX4_INFO("testing mathlib");
-
{
Vector<2> v;
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>", v1 % v2);
}
+ return true;
+}
+
+bool MathlibTest::testVector3(void)
+{
{
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 direct", v1.data[0] = 1.0f);
}
+ return true;
+}
+bool MathlibTest::testVector4(void)
+{
{
Vector<4> v;
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);
}
+ return true;
+}
+bool MathlibTest::testVector10(void)
+{
{
Vector<10> v1;
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>(float[])", Vector<10> v3(data));
}
+ return true;
+}
+bool MathlibTest::testMatrix3x3(void)
+{
{
Matrix<3, 3> m1;
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);
}
+ return true;
+}
+bool MathlibTest::testMatrix10x10(void)
+{
{
Matrix<10, 10> m1;
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);
}
+ return true;
+}
+bool MathlibTest::testMatrixNonsymmetric(void)
+{
+ int rc = true;
{
- PX4_INFO("Nonsymmetric matrix operations test");
+ //PX4_INFO("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -=
float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
(m1 + m2).print();
printf("!=\n");
m3.print();
- rc = 1;
+ rc = false;
}
+ ut_assert("m1 + m2 == m3", m1 + m2 == m3);
+
if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
(m3 - m2).print();
printf("!=\n");
m1.print();
- rc = 1;
+ rc = false;
}
+ ut_assert("m3 - m2 == m1", m3 - m2 == m1);
+
m1 += m2;
if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m3.print();
- rc = 1;
+ rc = false;
}
+ ut_assert("m1 == m3", m1 == m3);
+
m1 -= m2;
Matrix<2, 3> m1_orig(data1);
@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m1_orig.print();
- rc = 1;
+ rc = false;
}
+ ut_assert("m1 == m1_orig", m1 == m1_orig);
+
}
+ return rc;
+}
- {
- // 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;
- }
- }
+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);
}
}
}
}
+ }
- // 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);
+ return true;
+}
- 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};
+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};
- 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;
- }
- }
+ math::Matrix<3, 3> R_orig;
+ R_orig.identity();
- q_true.from_euler(-1.5f, -0.2f, 0.5f);
- q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
+ math::Quaternion q;
+ 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_euler()' outside tolerance!");
- rc = 1;
- }
- }
+ for (unsigned i = 0; i < 4; i++) {
+ ut_assert("Quaternion method 'from_dcm()' 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};
+ return true;
+}
- 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;
- }
- }
+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);
}
- {
- // 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;
- }
- }
+ 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++) {
- if (fabsf(vector_true(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};
- 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);
+ }
- 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};
- 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);
+ }
- 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++) {
+ 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};
+ 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;
- }
- }
+ for (unsigned i = 0; i < 3; i++) {
+ ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
- return rc;
+
+ 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)
diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp
new file mode 100644
index 0000000000..2ef48e0c4f
--- /dev/null
+++ b/src/systemcmds/tests/test_matrix.cpp
@@ -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;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 893a65a1b9..88c37d6e9c 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -85,22 +85,22 @@ int test_mixer(int argc, char *argv[])
uint16_t servo_predicted[output_max];
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";
+#else
+ const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
+#endif
- if (argc > 1) {
- filename = argv[1];
- }
-
- PX4_INFO("loading: %s", filename);
+ //PX4_INFO("loading: %s", filename);
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(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
* in the case of a remote load,
@@ -114,11 +114,7 @@ int test_mixer(int argc, char *argv[])
/* load at once test */
unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx);
- PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
-
- if (mixer_group.count() != 8) {
- return 1;
- }
+ //ASSERT_EQ(mixer_group.count(), 8);
unsigned empty_load = 2;
char empty_buf[2];
@@ -126,7 +122,9 @@ int test_mixer(int argc, char *argv[])
empty_buf[1] = '\0';
mixer_group.reset();
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) {
return 1;
@@ -140,7 +138,7 @@ int test_mixer(int argc, char *argv[])
unsigned transmitted = 0;
- PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
+ //PX4_INFO("transmitted: %d, loaded: %d", 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);
mixer_text_length += text_length;
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 */
unsigned resid = mixer_text_length;
@@ -163,7 +161,7 @@ int test_mixer(int argc, char *argv[])
/* if anything was parsed */
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 */
if (resid > 0) {
@@ -176,7 +174,7 @@ int test_mixer(int argc, char *argv[])
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) {
return 1;
@@ -198,7 +196,8 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
- PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+ //PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+
/* mix */
should_prearm = true;
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);
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 (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;
}
- PX4_INFO("ARMING TEST: STARTING RAMP");
+ //PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
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);
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 */
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++) {
@@ -292,7 +291,7 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max, outputs,
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++) {
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();
sleepcount = 0;
@@ -324,7 +323,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
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 */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -337,14 +336,14 @@ int test_mixer(int argc, char *argv[])
sleepcount++;
if (sleepcount % 10 == 0) {
- printf(".");
- fflush(stdout);
+ //printf(".");
+ //fflush(stdout);
}
}
- printf("\n");
+ //printf("\n");
- PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
+ //PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
@@ -366,7 +365,7 @@ int test_mixer(int argc, char *argv[])
/* 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 &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
@@ -388,46 +387,42 @@ int test_mixer(int argc, char *argv[])
sleepcount++;
if (sleepcount % 10 == 0) {
- printf(".");
- fflush(stdout);
+ // printf(".");
+ // fflush(stdout);
}
}
- printf("\n");
+ //printf("\n");
/* load multirotor at once test */
mixer_group.reset();
- if (argc > 2) {
- filename = argv[2];
-
- } else {
- filename = "/etc/mixers/quad_test.mix";
- }
+#if !defined(CONFIG_ARCH_BOARD_SITL)
+ filename = "/etc/mixers/quad_test.mix";
+#else
+ filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
+#endif
load_mixer_file(filename, &buf[0], sizeof(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;
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) {
PX4_ERR("FAIL: Quad test mixer load failed");
return 1;
}
- PX4_INFO("SUCCESS: No errors in mixer test");
+ //PX4_INFO("SUCCESS: No errors in mixer test");
return 0;
}
static int
-mixer_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &control)
+mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
{
if (control_group != 0) {
return -1;
diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/src/systemcmds/tests/test_perf.c
similarity index 68%
rename from unittests/uorb_unittests/uORBGtestTopics.hpp
rename to src/systemcmds/tests/test_perf.c
index ca8572efac..89be5000b2 100644
--- a/unittests/uorb_unittests/uORBGtestTopics.hpp
+++ b/src/systemcmds/tests/test_perf.c
@@ -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
* modification, are permitted provided that the following conditions
@@ -31,28 +31,39 @@
*
****************************************************************************/
-#ifndef _uORBGtestTopics_hpp_
-#define _uORBGtestTopics_hpp_
+#include <px4_config.h>
+#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
- {
- int16_t val;
- };
+ perf_counter_t cc = perf_alloc(PC_COUNT, "test_count");
+ perf_counter_t ec = perf_alloc(PC_ELAPSED, "test_elapsed");
- struct orb_topic_B
- {
- int16_t val;
- };
+ if ((cc == NULL) || (ec == NULL)) {
+ printf("perf: counter alloc failed\n");
+ return 1;
+ }
- ORB_DEFINE(topicA, struct orb_topic_A, nullptr, "TOPICA:int16 val;");
- ORB_DEFINE(topicB, struct orb_topic_B, nullptr, "TOPICB:int16 val;");
+ 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);
- ORB_DEFINE(topicA_clone, struct orb_topic_A, nullptr, "TOPICA_CLONE:int16 val;";
- ORB_DEFINE(topicB_clone, struct orb_topic_B, nullptr, "TOPICB_CLONE:int16 val;");
-}
+ perf_free(cc);
+ perf_free(ec);
-#endif // _UnitTest_uORBTopics_hpp_
+ return OK;
+}
diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
index 7c138efca1..7156543021 100644
--- a/src/systemcmds/tests/test_uart_baudchange.c
+++ b/src/systemcmds/tests/test_uart_baudchange.c
@@ -57,40 +57,6 @@
#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
- ****************************************************************************/
-
int test_uart_baudchange(int argc, char *argv[])
{
int uart2_nwrite = 0;
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
index 682ad5a129..dc1f59c57d 100644
--- a/src/systemcmds/tests/test_uart_console.c
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -56,40 +56,6 @@
#include <float.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)
{
int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 552a0c2d6b..2f5004c36c 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -70,50 +70,48 @@
# endif
#endif
-/****************************************************************************
- * Public Types
- ****************************************************************************/
-
-/****************************************************************************
- * Public Variables
- ****************************************************************************/
-
-/****************************************************************************
- * Public Function Prototypes
- ****************************************************************************/
-
__BEGIN_DECLS
-extern int test_sensors(int argc, char *argv[]);
+extern int test_adc(int argc, char *argv[]);
+extern int test_autodeclination(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_file2(int argc, char *argv[]);
+extern int test_float(int argc, char *argv[]);
extern int test_gpio(int argc, char *argv[]);
+extern int test_hott_telemetry(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_int(int argc, char *argv[]);
-extern int test_float(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_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_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_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_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_file(int argc, char *argv[]);
-extern int test_file2(int argc, char *argv[]);
-extern int test_mixer(int argc, char *argv[]);
-extern int test_rc(int argc, char *argv[]);
-extern int test_conv(int argc, char *argv[]);
-extern int test_mount(int argc, char *argv[]);
-extern int test_mathlib(int argc, char *argv[]);
-extern int test_eigen(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
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index e37a823e13..a733c4eada 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -38,20 +38,17 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
+#include "tests.h"
+
#include <px4_config.h>
#include <sys/types.h>
#include <stdio.h>
-#include <stdlib.h>
#include <string.h>
-#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
-#include <arch/board/board.h>
-#include <systemlib/perf_counter.h>
-
// Not using Eigen at the moment
#define TESTS_EIGEN_DISABLE
@@ -62,8 +59,9 @@
****************************************************************************/
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_perf(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_NOJIGTEST (1<<2)
} tests[] = {
+ {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
+ {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST},
+ {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST},
#ifdef __PX4_NUTTX
+ {"adc", test_adc, OPT_NOJIGTEST},
{"led", test_led, 0},
- {"time", test_time, OPT_NOJIGTEST},
{"sensors", test_sensors, 0},
- {"adc", test_adc, OPT_NOJIGTEST},
+ {"time", test_time, OPT_NOJIGTEST},
+ {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST},
+#else
+ {"rc", rc_tests_main, 0},
#endif /* __PX4_NUTTX */
- {"int", test_int, 0},
+
+ /* 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},
+ {"conv", test_conv, 0},
+ {"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST},
+ {"file2", test_file2, OPT_NOJIGTEST},
{"float", test_float, 0},
{"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST},
+ {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
+ {"int", test_int, 0},
+ {"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
+ {"mathlib", test_mathlib, 0},
+ {"matrix", test_matrix, 0},
+ {"mixer", test_mixer, OPT_NOJIGTEST},
+ {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
+ {"param", test_param, 0},
+ {"perf", test_perf, OPT_NOJIGTEST},
{"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},
+ {"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_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},
- {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST},
- {"param", test_param, 0},
- {"bson", test_bson, 0},
- {"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST},
- {"file2", test_file2, OPT_NOJIGTEST},
- {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
- {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
- {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
- {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
-#ifndef TESTS_MATHLIB_DISABLE
- {"mathlib", test_mathlib, 0},
-#endif
-#ifndef TESTS_EIGEN_DISABLE
- {"eigen", test_eigen, OPT_NOJIGTEST},
-#endif
- {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}
};
@@ -139,142 +144,29 @@ test_help(int argc, char *argv[])
static int
test_all(int argc, char *argv[])
{
- unsigned i;
- char *args[2] = {"all", 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_NOALLTEST)) {
- 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\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"); }
-
- unsigned int k;
-
- 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);
- }
- }
-
- fflush(stdout);
-
- return (failcount > 0);
+ return test_runner(OPT_NOALLTEST);
}
static int
-test_perf(int argc, char *argv[])
+test_jig(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;
+ return test_runner(OPT_NOJIGTEST);
}
-int test_jig(int argc, char *argv[])
+static int
+test_runner(unsigned option)
{
unsigned i;
- char *args[2] = {"jig", NULL};
+ char *args[2] = {"all", NULL};
unsigned int failcount = 0;
unsigned int testcount = 0;
- bool passed[NTESTS];
+ unsigned 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)) {
+ if (!(tests[i].options & option)) {
for (int j = 0; j < 80; j++) {
printf("-");
}
@@ -289,19 +181,19 @@ int test_jig(int argc, char *argv[])
fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name);
fflush(stderr);
failcount++;
- passed[i] = false;
+ passed[i] = 0;
} else {
printf(" [%s] \t\tPASS\n", tests[i].name);
fflush(stdout);
- passed[i] = true;
+ passed[i] = 1;
}
for (int j = 0; j < 80; j++) {
printf("-");
}
- printf("\n");
+ printf("\n\n");
testcount++;
}
@@ -310,8 +202,8 @@ int test_jig(int argc, char *argv[])
/* Print summary */
printf("\n");
- for (int j = 0; j < 80; j++) {
- printf("-");
+ for (unsigned j = 0; j < 80; j++) {
+ printf("#");
}
printf("\n\n");
@@ -340,17 +232,19 @@ int test_jig(int argc, char *argv[])
printf("\n");
/* Print failed tests */
- if (failcount > 0) { printf(" Failed tests:\n\n"); }
+ if (failcount > 0) {
+ printf(" Failed tests:\n\n");
+ }
- for (int k = 0; k < i; k++) {
- if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
+ for (unsigned k = 0; k < i; k++) {
+ if (!passed[k] && !(tests[k].options & option)) {
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;
+ return (failcount > 0);
}
__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[])
{
- unsigned i;
-
if (argc < 2) {
printf("tests: missing test name - 'tests help' for a list of tests\n");
return 1;
}
- for (i = 0; tests[i].name; i++) {
+ for (unsigned i = 0; tests[i].name; i++) {
if (!strcmp(tests[i].name, 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]);
- return ERROR;
+ return 1;
}
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index c2e11ea37a..9df3adcce3 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,9 +1,5 @@
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")
add_compile_options(-Qunused-arguments )
endif()
@@ -14,28 +10,16 @@ endif()
project(unittests)
enable_testing()
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-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_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
-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)
add_subdirectory(${GTEST_DIR})
include_directories(${GTEST_DIR}/include)
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(${PX4_SITL_BUILD}/src)
@@ -52,38 +36,27 @@ include_directories(${PX4_SRC}/modules/uORB)
include_directories(${PX4_SRC}/platforms)
include_directories(${PX4_SRC}/platforms/posix/include)
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__PX4_POSIX)
add_definitions(-D__PX4_TESTS)
+add_definitions(-D__PX4_UNIT_TESTS)
add_definitions(-D_UNIT_TEST=)
add_definitions(-DERROR=-1)
add_definitions(-Dmain_t=int)
add_definitions(-Dnoreturn_function=)
add_definitions(-DOK=0)
-# check
-add_custom_target(check
- COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
- WORKING_DIR ${CMAKE_BINARY_DIR}
- USES_TERMINAL)
-
-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()
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+ add_definitions(-D__PX4_DARWIN)
+else()
+ add_definitions(-D__PX4_LINUX)
+endif()
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/i2c_posix.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_queue.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_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
add_executable(param_test param_test.cpp uorb_stub.cpp
${PX4_SRC}/modules/systemlib/bson/tinybson.c
${PX4_SRC}/modules/systemlib/param/param.c)
target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
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)
diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp
deleted file mode 100644
index 8c89243f6e..0000000000
--- a/unittests/autodeclination_test.cpp
+++ /dev/null
@@ -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";
-}
diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp
deleted file mode 100644
index 7a2909dc34..0000000000
--- a/unittests/conversion_test.cpp
+++ /dev/null
@@ -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";
-}
diff --git a/unittests/data/fit_linear_voltage.m b/unittests/data/fit_linear_voltage.m
deleted file mode 100644
index 7d0c2c27fc..0000000000
--- a/unittests/data/fit_linear_voltage.m
+++ /dev/null
@@ -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)
diff --git a/unittests/data/px4io_v1.3.csv b/unittests/data/px4io_v1.3.csv
deleted file mode 100644
index b41ee8f1f9..0000000000
--- a/unittests/data/px4io_v1.3.csv
+++ /dev/null
@@ -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
diff --git a/unittests/dsm_test.cpp b/unittests/dsm_test.cpp
deleted file mode 100644
index 7bd6558139..0000000000
--- a/unittests/dsm_test.cpp
+++ /dev/null
@@ -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);
-}
diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp
deleted file mode 100644
index 73df8c54f2..0000000000
--- a/unittests/mixer_test.cpp
+++ /dev/null
@@ -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";
-}
diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp
deleted file mode 100644
index 828f2a6160..0000000000
--- a/unittests/sbus2_test.cpp
+++ /dev/null
@@ -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);
-}
diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp
deleted file mode 100644
index 5b6a22fd27..0000000000
--- a/unittests/st24_test.cpp
+++ /dev/null
@@ -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);
-}
diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp
deleted file mode 100644
index 40be7a9d8f..0000000000
--- a/unittests/sumd_test.cpp
+++ /dev/null
@@ -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);
-}
diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp
index e4269afc32..cc1926d69c 100644
--- a/unittests/uorb_stub.cpp
+++ b/unittests/uorb_stub.cpp
@@ -19,5 +19,3 @@ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void
{
return 0;
}
-
-
diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp
deleted file mode 100644
index f964c095e6..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMock.cpp
+++ /dev/null
@@ -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 ];
-}
diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp
deleted file mode 100644
index acffbdb7ab..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp
+++ /dev/null
@@ -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_ */
diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
deleted file mode 100644
index 8bb4260590..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
deleted file mode 100644
index 96a6375bb4..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
+++ /dev/null
@@ -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_ */
diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
deleted file mode 100644
index a0b61a250f..0000000000
--- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
+++ /dev/null
@@ -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 );
-
- }
-}
-
-
-
-
--
GitLab