diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 614dda71be..09f9ebffbb 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_git_submodule(TARGET git_matrix PATH "matrix") add_subdirectory(airspeed) add_subdirectory(airspeed_validator) +add_subdirectory(avoidance) add_subdirectory(battery) add_subdirectory(bezier) add_subdirectory(cdev) diff --git a/src/lib/avoidance/CMakeLists.txt b/src/lib/avoidance/CMakeLists.txt new file mode 100644 index 0000000000..c1da2a76dc --- /dev/null +++ b/src/lib/avoidance/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2020 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_library(avoidance + ObstacleAvoidance.cpp +) +target_link_libraries(avoidance PUBLIC hysteresis bezier) + +px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp rename to src/lib/avoidance/ObstacleAvoidance.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp rename to src/lib/avoidance/ObstacleAvoidance.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp similarity index 99% rename from src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp rename to src/lib/avoidance/ObstacleAvoidanceTest.cpp index 2c8506e545..bd6a9e99b2 100644 --- a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp +++ b/src/lib/avoidance/ObstacleAvoidanceTest.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ #include -#include +#include #include using namespace matrix; -// to run: make tests TESTFILTER=CollisionPrevention +// to run: make tests TESTFILTER=ObstacleAvoidance class ObstacleAvoidanceTest : public ::testing::Test { diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/lib/avoidance/ObstacleAvoidance_dummy.hpp new file mode 100644 index 0000000000..7831c919fd --- /dev/null +++ b/src/lib/avoidance/ObstacleAvoidance_dummy.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ObstacleAvoidance_dummy.hpp + * This is a dummy class to reduce flash space for when obstacle avoidance is not required + * + * @author Julian Kent + */ + +#pragma once + +#include +#include + + +#include + + +class ObstacleAvoidance +{ +public: + ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor + + + void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, + float &yaw_speed_sp) + { + notify_dummy(); + }; + + void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, + const float curr_yawspeed, + const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, + const int wp_type) + { + notify_dummy(); + }; + + void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, + const int type) + { + notify_dummy(); + } + + + void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, + float target_acceptance_radius, const matrix::Vector2f &closest_pt) + { + notify_dummy(); + }; + +protected: + + bool _logged_error = false; + void notify_dummy() + { + if (!_logged_error) { + PX4_ERR("Dummy avoidance library called!"); + _logged_error = true; + } + } +}; diff --git a/src/lib/flight_tasks/tasks/Auto/CMakeLists.txt b/src/lib/flight_tasks/tasks/Auto/CMakeLists.txt index b59931cc85..3d0bc84460 100644 --- a/src/lib/flight_tasks/tasks/Auto/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility) +target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp index 5e3ee4e879..838589c1a3 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp @@ -46,7 +46,13 @@ #include #include #include -#include + +// TODO: make this switchable in the board config, like a module +#if CONSTRAINED_FLASH +#include +#else +#include +#endif /** * This enum has to agree with position_setpoint_s type definition diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 93ff3062d9..96dc2e818d 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_library(FlightTaskUtility ManualSmoothingZ.cpp ManualSmoothingXY.cpp - ObstacleAvoidance.cpp StraightLine.cpp VelocitySmoothing.cpp ManualVelocitySmoothingXY.cpp @@ -46,4 +45,3 @@ target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility) px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility) -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS FlightTaskUtility)