Pre-emptive geofence triggering

cmake/gtest/px4_add_gtest.cmake

@@ -90,10 +90,9 @@ function(px4_add_functional_gtest)
 
 		# link the libary to test and gtest
 		target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_functional_main
-		                                              px4_daemon
+		                                              px4_layer
 		                                              px4_platform
 		                                              modules__uORB
-		                                              px4_layer
 		                                              systemlib
 		                                              cdev
 		                                              px4_work_queue

src/lib/CMakeLists.txt

@@ -54,6 +54,7 @@ add_subdirectory(led)
 add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)
+add_subdirectory(motion_planning)
 add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -41,7 +41,7 @@
 #pragma once
 
 #include "FlightTaskAutoMapper.hpp"
-#include "VelocitySmoothing.hpp"
+#include <motion_planning/VelocitySmoothing.hpp>
 
 class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
 {

src/lib/flight_tasks/tasks/Utility/CMakeLists.txt

@@ -33,16 +33,14 @@
 
 px4_add_library(FlightTaskUtility
 	StraightLine.cpp
-	VelocitySmoothing.cpp
 	ManualVelocitySmoothingXY.cpp
 	ManualVelocitySmoothingZ.cpp
 	Sticks.cpp
 	StickAccelerationXY.cpp
 	StickYaw.cpp
 )
 
-target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate)
+target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning)
 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)

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp

@@ -38,7 +38,7 @@
 
 #pragma once
 
-#include "VelocitySmoothing.hpp"
+#include <motion_planning/VelocitySmoothing.hpp>
 
 #include <matrix/matrix/math.hpp>
 

src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp

@@ -38,7 +38,7 @@
 
 #pragma once
 
-#include "VelocitySmoothing.hpp"
+#include <motion_planning/VelocitySmoothing.hpp>
 
 class ManualVelocitySmoothingZ final
 {

src/lib/mathlib/math/TrajMath.hpp

@@ -91,5 +91,24 @@ inline float computeMaxSpeedInWaypoint(const float alpha, const float accel, con
 
 	return max_speed_in_turn;
 }
+
+/* Compute the braking distance given a maximum acceleration, maximum jerk and a maximum delay acceleration.
+ * We assume a constant acceleration profile with a delay of accel_delay_max/jerk
+ * (time to reach the desired acceleration from opposite max acceleration)
+ * Equation to solve: vel_final^2 = vel_initial^2 - 2*accel*(x - vel_initial*2*accel/jerk)
+ *
+ * @param velocity initial velocity
+ * @param jerk maximum jerk
+ * @param accel maximum target acceleration during the braking maneuver
+ * @param accel_delay_max the acceleration defining the delay described above
+ *
+ * @return braking distance
+ */
+inline float computeBrakingDistanceFromVelocity(const float velocity, const float jerk, const float accel,
+		const float accel_delay_max)
+{
+	return velocity * (velocity / (2.0f * accel) + accel_delay_max / jerk);
+}
+
 } /* namespace traj */
 } /* namespace math */

src/lib/motion_planning/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2018 - 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(motion_planning
+	VelocitySmoothing.cpp
+)
+
+target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)

src/modules/navigator/CMakeLists.txt

@@ -31,6 +31,8 @@
 #
 ############################################################################
 
+add_subdirectory(GeofenceBreachAvoidance)
+
 px4_add_module(
 	MODULE modules__navigator
 	MAIN navigator
@@ -53,4 +55,6 @@ px4_add_module(
 		git_ecl
 		ecl_geo
 		landing_slope
-	)
+		geofence_breach_avoidance
+		motion_planning
+	)

src/modules/navigator/GeofenceBreachAvoidance/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(geofence_breach_avoidance
+	geofence_breach_avoidance.cpp
+	geofence_breach_avoidance.h
+)
+
+px4_add_functional_gtest(SRC GeofenceBreachAvoidanceTest.cpp LINKLIBS geofence_breach_avoidance modules__navigator ecl_geo motion_planning)