Provide a dummy avoidance for flash constrained boards

Part of this: move ObstacleAvoidance to a library

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)

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)

src/lib/avoidance/ObstacleAvoidanceTest.cpp

@@ -32,12 +32,12 @@
  ****************************************************************************/
 
 #include <gtest/gtest.h>
-#include <ObstacleAvoidance.hpp>
+#include <avoidance/ObstacleAvoidance.hpp>
 #include <uORB/Subscription.hpp>
 
 
 using namespace matrix;
-// to run: make tests TESTFILTER=CollisionPrevention
+// to run: make tests TESTFILTER=ObstacleAvoidance
 
 class ObstacleAvoidanceTest : public ::testing::Test
 {

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 <px4_platform_common/defines.h>
+#include <commander/px4_custom_mode.h>
+
+
+#include <matrix/matrix/math.hpp>
+
+
+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;
+		}
+	}
+};

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})

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp

@@ -46,7 +46,13 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/ecl/geo/geo.h>
-#include <ObstacleAvoidance.hpp>
+
+// TODO: make this switchable in the board config, like a module
+#if CONSTRAINED_FLASH
+#include <lib/avoidance/ObstacleAvoidance_dummy.hpp>
+#else
+#include <lib/avoidance/ObstacleAvoidance.hpp>
+#endif
 
 /**
  * This enum has to agree with position_setpoint_s type definition

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)