diff --git a/.gitignore b/.gitignore index d4d5a9d446..a458167565 100644 --- a/.gitignore +++ b/.gitignore @@ -64,3 +64,7 @@ posix-configs/SITL/init/test/*_generated *.gcov .coverage .coverage.* + +# KDevelop ignores +.kdev4/* +*.kdev4 diff --git a/Tools/run-gtest-isolated.py b/Tools/run-gtest-isolated.py new file mode 100755 index 0000000000..fe7290371d --- /dev/null +++ b/Tools/run-gtest-isolated.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python + +from sys import argv, stderr +from subprocess import call, check_output + + +def list_all_tests(bin_path): + out = check_output(['./' + bin_path,"--gtest_list_tests"]) + out = out.split("\n") + if len(out) > 1: + prefix = '' + tests_list = [] + for token in out: + if len(token) == 0: + continue + if token[-1] == '.': + prefix = token + continue + if token[0] == ' ' and token[1] == ' ' and len(token) > 0: + tests_list.append(''.join([prefix, token[2:]])) + continue + + return tests_list + + else: + return [] + + + +def run_tests(bin_path, tests_list, args): + for test in tests_list: + call_args = ['./' + bin_path, ' '.join(args), '--gtest_filter=*{}'.format(test)] + print(' '.join(call_args)) + result = call(call_args) + if result != 0: + return result + return 0 + + + +def main(): + if len(argv) >= 2: + try: + exit(run_tests(argv[1], list_all_tests(argv[1]), argv[2:])) + except: + print('Error with arguments "' + ' '.join(argv[1:]) + '"') + raise + print('Runs each gtest test in a difference process') + print('Usage: ' + argv[0] + ' path/to/gtest.bin [GTEST_OPTIONS]') + exit(1) + + +if __name__ == '__main__': + main() diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 4ae03387db..a69bdaf57d 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -39,12 +39,12 @@ include(px4_base) # # Adds a googletest unit test to the test_results target. # -function(px4_add_gtest) +function(px4_add_unit_gtest) # skip if unit testing is not configured if(BUILD_TESTING) # parse source file and library dependencies from arguments px4_parse_function_args( - NAME px4_add_gtest + NAME px4_add_unit_gtest ONE_VALUE SRC MULTI_VALUE LINKLIBS REQUIRED SRC @@ -62,7 +62,57 @@ function(px4_add_gtest) target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_main) # add the test to the ctest plan - add_test(NAME ${TESTNAME} COMMAND ${TESTNAME}) + add_test(NAME ${TESTNAME} + COMMAND ${TESTNAME} + WORKING_DIRECTORY ${PX4_BINARY_DIR}) + + # attach it to the unit test target + add_dependencies(test_results ${TESTNAME}) + endif() +endfunction() + +function(px4_add_functional_gtest) + # skip if unit testing is not configured + if(BUILD_TESTING) + # parse source file and library dependencies from arguments + px4_parse_function_args( + NAME px4_add_functional_gtest + ONE_VALUE SRC + MULTI_VALUE LINKLIBS + REQUIRED SRC + ARGN ${ARGN}) + + # infer test name from source filname + get_filename_component(TESTNAME ${SRC} NAME_WE) + string(REPLACE Test "" TESTNAME ${TESTNAME}) + set(TESTNAME functional-${TESTNAME}) + + # build a binary for the unit test + add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC}) + + # link the libary to test and gtest + target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_main + px4_daemon + px4_platform + modules__uORB + px4_layer + systemlib + cdev + px4_work_queue + px4_daemon + work_queue + parameters + perf + tinybson + uorb_msgs + test_stubs) #put test_stubs last + + # add the test to the ctest plan + add_test(NAME ${TESTNAME} + # functional tests need to run in a new process for each test, + # since they set up and tear down system components + COMMAND ${PX4_SOURCE_DIR}/Tools/run-gtest-isolated.py ${TESTNAME} + WORKING_DIRECTORY ${PX4_BINARY_DIR}) # attach it to the unit test target add_dependencies(test_results ${TESTNAME}) diff --git a/platforms/posix/src/lockstep_scheduler/CMakeLists.txt b/platforms/posix/src/lockstep_scheduler/CMakeLists.txt index 0705c07278..da84323d9d 100644 --- a/platforms/posix/src/lockstep_scheduler/CMakeLists.txt +++ b/platforms/posix/src/lockstep_scheduler/CMakeLists.txt @@ -10,4 +10,4 @@ target_include_directories(lockstep_scheduler ${CMAKE_CURRENT_SOURCE_DIR}/include ) -px4_add_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler) +px4_add_unit_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler) diff --git a/src/lib/CollisionPrevention/CMakeLists.txt b/src/lib/CollisionPrevention/CMakeLists.txt index ae36bf94b6..69c5bf9831 100644 --- a/src/lib/CollisionPrevention/CMakeLists.txt +++ b/src/lib/CollisionPrevention/CMakeLists.txt @@ -33,3 +33,5 @@ px4_add_library(CollisionPrevention CollisionPrevention.cpp) target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable + +px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention ) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index c64be5bc30..d01a6b6517 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -53,6 +53,18 @@ CollisionPrevention::~CollisionPrevention() if (_mavlink_log_pub != nullptr) { orb_unadvertise(_mavlink_log_pub); } + + if (_constraints_pub != nullptr) { + orb_unadvertise(_constraints_pub); + } + + if (_obstacle_distance_pub != nullptr) { + orb_unadvertise(_obstacle_distance_pub); + } + + if (_pub_vehicle_command != nullptr) { + orb_unadvertise(_pub_vehicle_command); + } } void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint, @@ -102,7 +114,7 @@ void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &o void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) { for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - distance_sensor_s distance_sensor; + distance_sensor_s distance_sensor {}; _sub_distance_sensor[i].copy(&distance_sensor); // consider only instaces with updated, valid data and orientations useful for collision prevention diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp new file mode 100644 index 0000000000..a96c06f9a1 --- /dev/null +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +// to run: make tests TESTFILTER=CollisionPrevention + +class CollisionPreventionTest : public ::testing::Test +{ +public: + void SetUp() override + { + uORB::Manager::initialize(); + param_init(); + } + + void TearDown() override + { + param_reset_all(); + uORB::Manager::terminate(); + } +}; + +TEST_F(CollisionPreventionTest, instantiation) { CollisionPrevention cp(nullptr); } + +TEST_F(CollisionPreventionTest, behaviorOff) +{ + // GIVEN: a simple setup condition + CollisionPrevention cp(nullptr); + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3.f; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // WHEN: we check if the setpoint should be modified + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + // THEN: it should be the same + EXPECT_EQ(original_setpoint, modified_setpoint); +} + +TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing) +{ + // GIVEN: a simple setup condition + CollisionPrevention cp(nullptr); + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3.f; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + + + // WHEN: we set the parameter check then apply the setpoint modification + float value = 10; // try to keep 10m away from obstacles + param_set(param, &value); + + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + // THEN: it shouldn't interfere with the setpoint, because there isn't an obstacle + EXPECT_EQ(original_setpoint, modified_setpoint); +} + +TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle) +{ + // GIVEN: a simple setup condition + CollisionPrevention cp(nullptr); + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 10; // try to keep 10m distance + param_set(param, &value); + + // AND: an obstacle message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.min_distance = 100; + message.max_distance = 1000; + message.timestamp = hrt_absolute_time(); + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + + for (int i = 0; i < distances_array_size; i++) { + message.distances[i] = 101; + } + + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + orb_unadvertise(obstacle_distance_pub); + + // THEN: it should be cut down a lot + EXPECT_GT(original_setpoint.norm() * 0.5f, modified_setpoint.norm()); //FIXME: this should actually be constrained to 0 +} diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index 78f6282dec..6955cdffc1 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -395,4 +395,27 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } +int CDev::unregister_driver_and_memory() +{ + int retval = PX4_OK; + + if (_registered) { + unregister_driver(_devname); + _registered = false; + + } else { + retval = -ENODEV; + } + + if (_devname != nullptr) { + free((void *)_devname); + _devname = nullptr; + + } else { + retval = -ENODEV; + } + + return retval; +} + } // namespace cdev diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index af036e8baa..f51ed45a3d 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -264,6 +264,17 @@ protected: px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ + + /** + * First, unregisters the driver. Next, free the memory for the devname, + * in case it was expected to have ownership. Sets devname to nullptr. + * + * This is only needed if the ownership of the devname was passed to the CDev, otherwise ~CDev handles it. + * + * @return PX4_OK on success, -ENODEV if the devname is already nullptr + */ + int unregister_driver_and_memory(); + private: const char *_devname{nullptr}; /**< device node name */ diff --git a/src/lib/hysteresis/CMakeLists.txt b/src/lib/hysteresis/CMakeLists.txt index 1943cdc1aa..bd71aa5ef6 100644 --- a/src/lib/hysteresis/CMakeLists.txt +++ b/src/lib/hysteresis/CMakeLists.txt @@ -33,4 +33,4 @@ px4_add_library(hysteresis hysteresis.cpp) -px4_add_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) +px4_add_unit_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index c31fad719e..5293f723bb 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -162,3 +162,5 @@ add_dependencies(parameters prebuild_targets) if(${PX4_PLATFORM} STREQUAL "nuttx") target_link_libraries(parameters PRIVATE flashparams tinybson) endif() + +px4_add_functional_gtest(SRC ParameterTest.cpp LINKLIBS parameters) diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp new file mode 100644 index 0000000000..491fadc615 --- /dev/null +++ b/src/lib/parameters/ParameterTest.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include + +class ParameterTest : public ::testing::Test +{ +public: + void SetUp() override + { + uORB::Manager::initialize(); + param_init(); + } + + void TearDown() override + { + param_reset_all(); + uORB::Manager::terminate(); + } +}; + + +TEST_F(ParameterTest, testParamReadWrite) +{ + // GIVEN a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + + // WHEN: we get the parameter + float value = -999.f; + int status = param_get(param, &value); + + // THEN it should be successful and have the default value + EXPECT_EQ(0, status); + EXPECT_EQ(-1, value); + + // WHEN: we set the parameter + value = 42.f; + status = param_set(param, &value); + + // THEN: it should be successful + EXPECT_EQ(0, status); + + // WHEN: we get the parameter again + float value2 = -1999.f; + status = param_get(param, &value2); + + // THEN: it should be exactly the value we set + EXPECT_EQ(0, status); + EXPECT_EQ(42.f, value2); +} + + +TEST_F(ParameterTest, testUorbSendReceive) +{ + // GIVEN: a uOrb message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.min_distance = 1.f; + message.max_distance = 10.f; + + // AND: a subscriber + uORB::SubscriptionData sub_obstacle_distance{ORB_ID(obstacle_distance)}; + + // WHEN we send the message + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + ASSERT_TRUE(obstacle_distance_pub != nullptr); + + // THEN: the subscriber should receive the message + sub_obstacle_distance.update(); + const obstacle_distance_s &obstacle_distance = sub_obstacle_distance.get(); + + // AND: the values we got should be the same + EXPECT_EQ(message.timestamp, obstacle_distance.timestamp); + EXPECT_EQ(message.min_distance, obstacle_distance.min_distance); + EXPECT_EQ(message.max_distance, obstacle_distance.max_distance); + + // AND: all the bytes should be equal + EXPECT_EQ(0, memcmp(&message, &obstacle_distance, sizeof(message))); +} diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt index f4b1ace74a..84110b6d4e 100644 --- a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt +++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt @@ -39,4 +39,4 @@ target_include_directories(AttitudeControl ${CMAKE_CURRENT_SOURCE_DIR} ) -px4_add_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) +px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt index 164b175ed9..9c78d9f8cd 100644 --- a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -39,4 +39,4 @@ target_include_directories(Takeoff ${CMAKE_CURRENT_SOURCE_DIR} ) -px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 278f0f7255..84127955f4 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -102,10 +102,10 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in return -ENOMEM; } - /* construct the new node */ + /* construct the new node, passing the ownership of path to it */ uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); - /* if we didn't get a device, that's bad */ + /* if we didn't get a device, that's bad, free the path too */ if (node == nullptr) { free((void *)devpath); return -ENOMEM; @@ -133,9 +133,6 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } } - /* also discard the name now */ - free((void *)devpath); - } else { // add to the node map;. _node_list.add(node); diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index c2f2c51d90..e152f89177 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -33,7 +33,6 @@ #include "uORBDeviceNode.hpp" -#include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -70,6 +69,8 @@ uORB::DeviceNode::~DeviceNode() if (_data != nullptr) { delete[] _data; } + + CDev::unregister_driver_and_memory(); } int diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index cad2672dd8..168293a356 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -56,7 +56,7 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode public: DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority, uint8_t queue_size = 1); - ~DeviceNode(); + virtual ~DeviceNode(); // no copy, assignment, move, move assignment DeviceNode(const DeviceNode &) = delete; diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 20c9bf8432..7febe89353 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -55,6 +55,17 @@ bool uORB::Manager::initialize() return _Instance != nullptr; } +bool uORB::Manager::terminate() +{ + if (_Instance != nullptr) { + delete _Instance; + _Instance = nullptr; + return true; + } + + return false; +} + uORB::Manager::Manager() { #ifdef ORB_USE_PUBLISHER_RULES diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 42d195b26f..1969e3000b 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -75,6 +75,12 @@ public: */ static bool initialize(); + /** + * Terminate the singleton. Call this after everything else. + * @return true on success + */ + static bool terminate(); + /** * Method to get the singleton instance for the uORB::Manager. * Make sure initialize() is called first. @@ -416,7 +422,7 @@ private: // data members private: //class methods Manager(); - ~Manager(); + virtual ~Manager(); #ifdef ORB_COMMUNICATOR /** diff --git a/src/platforms/CMakeLists.txt b/src/platforms/CMakeLists.txt index a2a39344fc..6055801eae 100644 --- a/src/platforms/CMakeLists.txt +++ b/src/platforms/CMakeLists.txt @@ -32,3 +32,7 @@ ############################################################################ add_subdirectory(common) + +if (${PX4_PLATFORM} STREQUAL "posix" AND BUILD_TESTING) + add_subdirectory(posix/test_stubs) +endif() diff --git a/src/platforms/apps.h.in b/src/platforms/apps.h.in index c3cc46cd44..37728e0fd9 100644 --- a/src/platforms/apps.h.in +++ b/src/platforms/apps.h.in @@ -4,6 +4,7 @@ #include "px4_tasks.h" // px4_main_t #include +#include // Maps an app name to it's function. typedef std::map apps_map_type; diff --git a/src/platforms/posix/test_stubs/CMakeLists.txt b/src/platforms/posix/test_stubs/CMakeLists.txt new file mode 100644 index 0000000000..1950cb8c8a --- /dev/null +++ b/src/platforms/posix/test_stubs/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +set(SRCS + stub_daemon.cpp + stub_devmgr.cpp + stub_parameter.cpp + ) + +px4_add_library(test_stubs ${SRCS}) +message("-- ADDING TEST STUBS") diff --git a/src/platforms/posix/test_stubs/stub_daemon.cpp b/src/platforms/posix/test_stubs/stub_daemon.cpp new file mode 100644 index 0000000000..d379f90d5e --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_daemon.cpp @@ -0,0 +1,6 @@ +#include "stub_daemon.h" + + +void init_app_map(apps_map_type &apps) {stub_init_app_map_callback(apps);} + +void list_builtins(apps_map_type &apps) {stub_list_builtins_callback(apps);} diff --git a/src/platforms/posix/test_stubs/stub_daemon.h b/src/platforms/posix/test_stubs/stub_daemon.h new file mode 100644 index 0000000000..34119b177d --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_daemon.h @@ -0,0 +1,8 @@ +#pragma once + +#include + +#include + +std::function stub_init_app_map_callback = [](apps_map_type &) {}; +std::function stub_list_builtins_callback = [](apps_map_type &) {}; diff --git a/src/platforms/posix/test_stubs/stub_devmgr.cpp b/src/platforms/posix/test_stubs/stub_devmgr.cpp new file mode 100644 index 0000000000..374712f7ba --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_devmgr.cpp @@ -0,0 +1,10 @@ +#include "stub_devmgr.h" + + +namespace DriverFramework +{ +int DevMgr::getNextDeviceName(unsigned int &index, const char **instancename) +{ + return stub_getNextDeviceName_callback(index, instancename); +} +} diff --git a/src/platforms/posix/test_stubs/stub_devmgr.h b/src/platforms/posix/test_stubs/stub_devmgr.h new file mode 100644 index 0000000000..15840c889f --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_devmgr.h @@ -0,0 +1,8 @@ +#pragma once + +#include "DevMgr.hpp" + +#include + +std::function stub_getNextDeviceName_callback = [](unsigned int &, +const char **) {return 0;}; diff --git a/src/platforms/posix/test_stubs/stub_parameter.cpp b/src/platforms/posix/test_stubs/stub_parameter.cpp new file mode 100644 index 0000000000..769d8d8010 --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_parameter.cpp @@ -0,0 +1,11 @@ +#include "stub_parameter.h" + + + +extern "C" { + /* This function blocks forever in tests, so override it with a version that can be customized */ + int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) + { + return stub_pthread_cond_wait_callback(cond, mutex); + } +} diff --git a/src/platforms/posix/test_stubs/stub_parameter.h b/src/platforms/posix/test_stubs/stub_parameter.h new file mode 100644 index 0000000000..57a709b547 --- /dev/null +++ b/src/platforms/posix/test_stubs/stub_parameter.h @@ -0,0 +1,8 @@ +#pragma once + +#include + +#include + +std::function stub_pthread_cond_wait_callback = +[](pthread_cond_t *, pthread_mutex_t *) {return 0;};