Add GeofenceBreachAvoidance class

Signed-off-by: Julian Kent <julian@auterion.com>

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

src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp

@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "geofence_breach_avoidance.h"
+#include "fake_geofence.hpp"
+
+using namespace matrix;
+using Vector2d = matrix::Vector2<double>;
+
+TEST(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
+{
+
+	GeofenceBreachAvoidance gf_avoidance;
+	struct map_projection_reference_s ref = {};
+	Vector2d home_global(42.1, 8.2);
+	map_projection_init(&ref, home_global(0), home_global(1));
+
+	Vector2d waypoint_north_east_local(1.0, 1.0);
+	waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
+	Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
+
+	float x, y;
+	map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y);
+	Vector2d waypoint_north_east_reprojected(x, y);
+
+	EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
+	EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
+
+
+	Vector2d waypoint_south_west_local = -waypoint_north_east_local;
+
+	Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
+
+	map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y);
+	Vector2d waypoint_south_west_reprojected(x, y);
+
+	EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
+	EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
+
+
+	Vector2d same_as_home_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 0.0);
+
+	EXPECT_EQ(home_global, same_as_home_global);
+}
+
+TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
+{
+	GeofenceBreachAvoidance gf_avoidance;
+	FakeGeofence geo;
+	struct map_projection_reference_s ref = {};
+	Vector2d home_global(42.1, 8.2);
+	map_projection_init(&ref, home_global(0), home_global(1));
+
+	geofence_violation_type_u gf_violation;
+	gf_violation.flags.fence_violation = true;
+
+	gf_avoidance.setTestPointDistance(20.0f);
+	gf_avoidance.setTestPointBearing(0.0f);
+	gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0);
+
+
+	Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
+
+	// the expected loiter point is located test_point_distance behind
+	Vector2d loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, -20.0f);
+
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
+
+
+	geo.setProbeFunctionBehavior(FakeGeofence::LEFT_INSIDE_RIGHT_OUTSIDE);
+	loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
+
+	loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f);
+
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
+
+	geo.setProbeFunctionBehavior(FakeGeofence::RIGHT_INSIDE_LEFT_OUTSIDE);
+	loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
+
+	loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f);
+
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
+
+	gf_violation.flags.fence_violation = false;
+	loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
+
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), home_global(0));
+	EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), home_global(1));
+}
+
+TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
+{
+	GeofenceBreachAvoidance gf_avoidance;
+	FakeGeofence geo;
+	struct map_projection_reference_s ref = {};
+	Vector2d home_global(42.1, 8.2);
+	map_projection_init(&ref, home_global(0), home_global(1));
+
+	param_t param = param_handle(px4::params::MPC_ACC_HOR);
+
+	float value = 3;
+	param_set(param, &value);
+
+	param = param_handle(px4::params::MPC_ACC_HOR_MAX);
+	value = 5;
+	param_set(param, &value);
+
+	param = param_handle(px4::params::MPC_JERK_AUTO);
+	value = 8;
+	param_set(param, &value);
+
+	geofence_violation_type_u gf_violation;
+	gf_violation.flags.fence_violation = true;
+
+	gf_avoidance.setTestPointDistance(30.0f);
+	gf_avoidance.setTestPointBearing(0.0f);
+	gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0);
+	// vehicle is approaching the fence at a crazy velocity
+	gf_avoidance.setHorizontalVelocity(1000.0f);
+	gf_avoidance.computeBrakingDistanceMultirotor();
+
+	geo.setProbeFunctionBehavior(FakeGeofence::GF_BOUNDARY_20M_AHEAD);
+
+	Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
+
+	Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f,
+					  20.0f - gf_avoidance.getMinDistToFenceMulticopter());
+
+	float error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0),
+			loiter_point_predicted(1));
+
+	EXPECT_LE(error, 0.5f);
+
+	// vehicle is approaching fenc slowly, plenty of time to brake
+	gf_avoidance.setHorizontalVelocity(0.1f);
+	gf_avoidance.computeBrakingDistanceMultirotor();
+
+	loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
+	loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f,
+				 gf_avoidance.computeBrakingDistanceMultirotor());
+
+
+	error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0),
+					      loiter_point_predicted(1));
+
+	EXPECT_LE(error, 0.0f);
+
+	gf_violation.flags.fence_violation = false;
+	loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
+
+	EXPECT_EQ(loiter_point, home_global);
+}
+
+TEST(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing)
+{
+	GeofenceBreachAvoidance gf_avoidance;
+	const float current_alt_amsl = 100.0f;
+	const float vertical_test_point_dist = 10.0f;
+
+	gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist);
+	gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude
+	geofence_violation_type_u gf_violation;
+	gf_violation.flags.max_altitude_exceeded = true;
+
+	float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation);
+
+	EXPECT_EQ(loiter_alt, current_alt_amsl - 2 * vertical_test_point_dist);
+
+	gf_violation.flags.max_altitude_exceeded = false;
+
+	loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation);
+
+	EXPECT_EQ(loiter_alt, current_alt_amsl);
+}
+
+TEST(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
+{
+	GeofenceBreachAvoidance gf_avoidance;
+	const float climbrate = 10.0f;
+	const float current_alt_amsl = 100.0f;
+	geofence_violation_type_u gf_violation;
+	gf_violation.flags.max_altitude_exceeded = true;
+
+
+	gf_avoidance.setClimbRate(climbrate);
+	gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl);
+	gf_avoidance.computeVerticalBrakingDistanceMultirotor();
+
+	float loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation);
+
+	EXPECT_EQ(loiter_alt_amsl, current_alt_amsl + gf_avoidance.computeVerticalBrakingDistanceMultirotor() -
+		  gf_avoidance.getMinVertDistToFenceMultirotor());
+
+	gf_violation.flags.max_altitude_exceeded = false;
+
+	loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation);
+
+	EXPECT_EQ(loiter_alt_amsl, current_alt_amsl);
+}

src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp

@@ -0,0 +1,183 @@
+
+
+
+#include"../geofence.h"
+#include <lib/mathlib/mathlib.h>
+
+class FakeGeofence : public Geofence
+{
+public:
+	FakeGeofence() :
+		Geofence(nullptr)
+	{};
+
+	virtual ~FakeGeofence() {};
+
+	virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude)
+	{
+		switch (_probe_function_behavior) {
+		case ALL_POINTS_OUTSIDE: {
+				return _allPointsOutside(lat, lon, altitude);
+			}
+
+		case LEFT_INSIDE_RIGHT_OUTSIDE: {
+				return _left_inside_right_outside(lat, lon, altitude);
+			}
+
+		case RIGHT_INSIDE_LEFT_OUTSIDE: {
+				return _right_inside_left_outside(lat, lon, altitude);
+			}
+
+		case GF_BOUNDARY_20M_AHEAD: {
+				return _gf_boundary_is_20m_ahead(lat, lon, altitude);
+			}
+
+		default:
+			return _allPointsOutside(lat, lon, altitude);
+		}
+	}
+
+	enum PROBE_FUNC_ENUM {
+		ALL_POINTS_OUTSIDE = 0,
+		LEFT_INSIDE_RIGHT_OUTSIDE,
+		RIGHT_INSIDE_LEFT_OUTSIDE,
+		GF_BOUNDARY_20M_AHEAD
+	};
+
+	void setProbeFunctionBehavior(PROBE_FUNC_ENUM func) {_probe_function_behavior = func;}
+
+
+private:
+
+	PROBE_FUNC_ENUM _probe_function_behavior = ALL_POINTS_OUTSIDE;
+
+	bool _allPointsOutside(double lat, double lon, float alt)
+	{
+		return false;
+	}
+
+	bool _left_inside_right_outside(double lat, double lon, float alt)
+	{
+		static int flag = true;
+
+		if (flag) {
+			flag = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
+	bool _right_inside_left_outside(double lat, double lon, float alt)
+	{
+		static int flag = false;
+
+		if (flag) {
+			flag = false;
+			return true;
+
+		} else {
+			flag = true;
+			return false;
+		}
+	}
+
+	bool _gf_boundary_is_20m_ahead(double lat, double lon, float alt)
+	{
+		struct map_projection_reference_s ref = {};
+		matrix::Vector2<double> home_global(42.1, 8.2);
+		map_projection_init(&ref, home_global(0), home_global(1));
+
+		float x, y;
+		map_projection_project(&ref, lat, lon, &x, &y);
+		matrix::Vector2f waypoint_local(x, y);
+
+		if (waypoint_local(0) >= 20.0f) {
+			return false;
+		}
+
+		return true;
+	}
+};
+
+typedef enum {
+	DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
+	DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
+	DM_PERSIST_VOLATILE             /* Data does not survive resets */
+} dm_persitence_t;
+
+typedef enum {
+	DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
+	DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
+	DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+	DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alternate between 0 and 1) */
+	DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+	DM_KEY_MISSION_STATE,		/* Persistent mission state */
+	DM_KEY_COMPAT,
+	DM_KEY_NUM_KEYS			/* Total number of item types defined */
+} dm_item_t;
+
+typedef enum {
+	DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
+	DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
+	DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
+} dm_reset_reason;
+
+extern "C" {
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned index,			/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	) {return 0;};
+
+	/** write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned index,			/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	) {return 0;};
+
+	/**
+	 * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
+	 * atomically).
+	 * Note that this lock is independent from dm_read & dm_write calls.
+	 * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
+	 */
+	__EXPORT int
+	dm_lock(
+		dm_item_t item			/* The item type to lock */
+	) {return 0;};
+
+	/**
+	 * Try to lock all items of a type (@see sem_trywait()).
+	 * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
+	 */
+	__EXPORT int
+	dm_trylock(
+		dm_item_t item			/* The item type to lock */
+	) {return 0;};
+
+	/** Unlock all items of a type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to unlock */
+	) {};
+
+	/** Erase all items of this type */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+	) {return 0;};
+
+	/** Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	) {return 0;};
+}

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp

@@ -0,0 +1,293 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "geofence_breach_avoidance.h"
+#include <lib/ecl/geo/geo.h>
+#include <motion_planning/VelocitySmoothing.hpp>
+
+using Vector2d = matrix::Vector2<double>;
+
+
+GeofenceBreachAvoidance::GeofenceBreachAvoidance() :
+	ModuleParams(nullptr)
+{
+	_paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX");
+	_paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR");
+	_paramHandle.param_mpc_acc_hor_max = param_find("MPC_ACC_HOR_MAX");
+	_paramHandle.param_mpc_jerk_auto = param_find("MPC_JERK_AUTO");
+	_paramHandle.param_mpc_acc_up_max = param_find("MPC_ACC_UP_MAX");
+	_paramHandle.param_mpc_acc_down_max = param_find("MPC_ACC_DOWN_MAX");
+
+	updateParameters();
+}
+
+GeofenceBreachAvoidance::~GeofenceBreachAvoidance()
+{
+
+}
+
+void GeofenceBreachAvoidance::updateParameters()
+{
+	param_get(_paramHandle.param_mpc_jerk_max, &_params.param_mpc_jerk_max);
+	param_get(_paramHandle.param_mpc_acc_hor, &_params.param_mpc_acc_hor);
+	param_get(_paramHandle.param_mpc_acc_hor_max, &_params.param_mpc_acc_hor_max);
+	param_get(_paramHandle.param_mpc_jerk_auto, &_params.param_mpc_jerk_auto);
+	param_get(_paramHandle.param_mpc_acc_up_max, &_params.param_mpc_acc_up_max);
+	param_get(_paramHandle.param_mpc_acc_down_max, &_params.param_mpc_acc_down_max);
+
+	updateMinHorDistToFenceMultirotor();
+}
+
+void GeofenceBreachAvoidance::setTestPointBearing(float test_point_bearing)
+{
+	_test_point_bearing = test_point_bearing;
+}
+
+void GeofenceBreachAvoidance::setTestPointDistance(float test_point_distance)
+{
+	_test_point_distance = test_point_distance;
+}
+
+void GeofenceBreachAvoidance::setVerticalTestPointDistance(float distance)
+{
+	_vertical_test_point_distance = distance;
+}
+
+void GeofenceBreachAvoidance::setHorizontalVelocity(float velocity_hor_abs)
+{
+	_velocity_hor_abs = velocity_hor_abs;
+}
+
+void GeofenceBreachAvoidance::setClimbRate(float climb_rate)
+{
+	_climbrate = climb_rate;
+}
+
+void GeofenceBreachAvoidance::setCurrentPosition(double lat, double lon, float alt)
+{
+	_current_pos_lat_lon = Vector2d(lat, lon);
+	_current_alt_amsl = alt;
+}
+
+matrix::Vector2<double> GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2<double>
+		current_pos_lat_lon, float test_point_bearing, float test_point_distance)
+{
+	// TODO: Remove this once the underlying geo function has been fixed
+	if (test_point_distance < 0.0f) {
+		test_point_distance *= -1.0f;
+		test_point_bearing = matrix::wrap_2pi(test_point_bearing + M_PI_F);
+	}
+
+	double fence_violation_test_point_lat, fence_violation_test_point_lon;
+	waypoint_from_heading_and_distance(current_pos_lat_lon(0), current_pos_lat_lon(1), test_point_bearing,
+					   test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
+
+	return Vector2d(fence_violation_test_point_lat, fence_violation_test_point_lon);
+}
+
+Vector2d
+GeofenceBreachAvoidance::getFenceViolationTestPoint()
+{
+	return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _test_point_distance);
+}
+
+Vector2d
+GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence)
+{
+	if (violation_type.flags.fence_violation) {
+		const float bearing_90_left = matrix::wrap_2pi(_test_point_bearing - M_PI_F * 0.5f);
+		const float bearing_90_right = matrix::wrap_2pi(_test_point_bearing + M_PI_F * 0.5f);
+
+		double loiter_center_lat, loiter_center_lon;
+		double fence_violation_test_point_lat, fence_violation_test_point_lon;
+
+		waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_left,
+						   _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
+
+		const bool left_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat,
+						       fence_violation_test_point_lon, _current_alt_amsl);
+
+		waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_right,
+						   _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
+
+		const bool right_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat,
+							fence_violation_test_point_lon, _current_alt_amsl);
+
+		float bearing_to_loiter_point;
+
+		if (right_side_is_inside_fence && !left_side_is_inside_fence) {
+			bearing_to_loiter_point = bearing_90_right;
+
+		} else if (left_side_is_inside_fence && !right_side_is_inside_fence) {
+			bearing_to_loiter_point = bearing_90_left;
+
+		} else {
+			bearing_to_loiter_point = matrix::wrap_2pi(_test_point_bearing + M_PI_F);
+		}
+
+		waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_to_loiter_point,
+						   _test_point_distance, &loiter_center_lat, &loiter_center_lon);
+
+		return Vector2d(loiter_center_lat, loiter_center_lon);
+
+	} else {
+		return _current_pos_lat_lon;
+	}
+}
+
+Vector2d
+GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence)
+{
+
+	if (violation_type.flags.fence_violation) {
+		float current_distance = _test_point_distance * 0.5f;
+		float current_min = 0.0f;
+		float current_max = _test_point_distance;
+		Vector2d test_point;
+
+		// logarithmic search for the distance from the drone to the geofence in the given direction
+		while (abs(current_max - current_min) > 0.5f) {
+			test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance);
+
+			if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) {
+				current_max = current_distance;
+				current_distance = current_min + (current_max - current_min) * 0.5f;
+
+			} else {
+				current_min = current_distance;
+				current_distance = current_min + (current_max - current_min) * 0.5f;
+			}
+		}
+
+		test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance);
+
+		if (_multirotor_braking_distance > current_distance - _min_hor_dist_to_fence_mc) {
+			return waypointFromBearingAndDistance(test_point, _test_point_bearing + M_PI_F, _min_hor_dist_to_fence_mc);
+
+		} else {
+			return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance);
+		}
+
+	} else {
+		if (_velocity_hor_abs > 0.5f) {
+			return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance);
+
+		} else {
+			return _current_pos_lat_lon;
+		}
+	}
+}
+
+float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type)
+{
+	if (violation_type.flags.max_altitude_exceeded) {
+		return _current_alt_amsl - 2.0f * _vertical_test_point_distance;
+
+	} else {
+		return _current_alt_amsl;
+	}
+}
+
+float GeofenceBreachAvoidance::generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type)
+{
+	if (violation_type.flags.max_altitude_exceeded) {
+		return _current_alt_amsl + _multirotor_vertical_braking_distance - _min_vert_dist_to_fence_mc;
+
+	} else {
+		return _current_alt_amsl;
+	}
+}
+
+float GeofenceBreachAvoidance::computeBrakingDistanceMultirotor()
+{
+	const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max);
+	VelocitySmoothing predictor(accel_delay_max, _velocity_hor_abs, 0.f);
+	predictor.setMaxVel(_velocity_hor_abs);
+	predictor.setMaxAccel(_params.param_mpc_acc_hor);
+	predictor.setMaxJerk(_params.param_mpc_jerk_auto);
+	predictor.updateDurations(0.f);
+	predictor.updateTraj(predictor.getTotalTime());
+
+	_multirotor_braking_distance =  predictor.getCurrentPosition() + 0.2f * _velocity_hor_abs; // navigator runs at 5Hz
+
+	return _multirotor_braking_distance;
+}
+
+float GeofenceBreachAvoidance::computeVerticalBrakingDistanceMultirotor()
+{
+	const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max);
+	const float vertical_vel_abs = fabsf(_climbrate);
+
+
+	VelocitySmoothing predictor(accel_delay_max, vertical_vel_abs, 0.f);
+	predictor.setMaxVel(vertical_vel_abs);
+	predictor.setMaxAccel(_climbrate > 0 ? _params.param_mpc_acc_down_max : _params.param_mpc_acc_up_max);
+	predictor.setMaxJerk(_params.param_mpc_jerk_auto);
+	predictor.updateDurations(0.f);
+	predictor.updateTraj(predictor.getTotalTime());
+
+	_multirotor_vertical_braking_distance =  predictor.getCurrentPosition() + 0.2f *
+			vertical_vel_abs; // navigator runs at 5Hz
+
+	_multirotor_vertical_braking_distance = matrix::sign(_climbrate) * _multirotor_vertical_braking_distance;
+
+	return _multirotor_vertical_braking_distance;
+}
+
+void GeofenceBreachAvoidance::updateMinHorDistToFenceMultirotor()
+{
+	const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max);
+	VelocitySmoothing predictor(accel_delay_max, 0.0f, 0.f);
+	predictor.setMaxVel(0.0f);
+	predictor.setMaxAccel(_params.param_mpc_acc_hor);
+	predictor.setMaxJerk(_params.param_mpc_jerk_auto);
+	predictor.updateDurations(0.f);
+	predictor.updateTraj(predictor.getTotalTime());
+
+	_min_hor_dist_to_fence_mc = 2.0f * predictor.getCurrentPosition();
+
+}
+
+void GeofenceBreachAvoidance::updateMinVertDistToFenceMultirotor()
+{
+	const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max);
+
+	VelocitySmoothing predictor(accel_delay_max, 0, 0.f);
+	predictor.setMaxVel(0);
+	predictor.setMaxAccel(_params.param_mpc_acc_down_max);
+	predictor.setMaxJerk(_params.param_mpc_jerk_auto);
+	predictor.updateDurations(0.f);
+	predictor.updateTraj(predictor.getTotalTime());
+
+	_min_vert_dist_to_fence_mc =  2.0f * predictor.getCurrentPosition();
+}

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <lib/mathlib/mathlib.h>
+#include <px4_platform_common/module_params.h>
+#include "../geofence.h"
+#include <px4_platform_common/defines.h>
+
+
+class Geofence;
+
+union geofence_violation_type_u {
+	struct {
+		bool dist_to_home_exceeded: 1;	///< 0 - distance to home exceeded
+		bool max_altitude_exceeded: 1;	///< 1 - maximum altitude exceeded
+		bool fence_violation: 1;	///< 2- violation of user defined fence
+	} flags;
+	uint8_t value;
+};
+
+class GeofenceBreachAvoidance : public ModuleParams
+{
+public:
+	GeofenceBreachAvoidance();
+
+	~GeofenceBreachAvoidance();
+
+	matrix::Vector2<double> waypointFromBearingAndDistance(matrix::Vector2<double> current_pos_lat_lon,
+			float test_point_bearing, float test_point_distance);
+
+	matrix::Vector2<double> getFenceViolationTestPoint();
+
+	matrix::Vector2<double>
+	generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence);
+
+	float computeBrakingDistanceMultirotor();
+
+	float computeVerticalBrakingDistanceMultirotor();
+
+	matrix::Vector2<double> generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence);
+
+	float generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type);
+
+	float generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type);
+
+	float getMinDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;}
+
+	float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;}
+
+	void setTestPointBearing(float test_point_bearing);
+
+	void setTestPointDistance(float test_point_distance);
+
+	void setVerticalTestPointDistance(float distance);
+
+	void setHorizontalVelocity(float velocity_hor_abs);
+
+	void setClimbRate(float climbrate);
+
+	void setCurrentPosition(double lat, double lon, float alt);
+
+	void updateParameters();
+
+
+private:
+	struct {
+		param_t param_mpc_jerk_max;
+		param_t param_mpc_acc_hor;
+		param_t param_mpc_acc_hor_max;
+		param_t param_mpc_jerk_auto;
+		param_t param_mpc_acc_up_max;
+		param_t param_mpc_acc_down_max;
+
+	} _paramHandle;
+
+	struct {
+		float param_mpc_jerk_max;
+		float param_mpc_acc_hor;
+		float param_mpc_acc_hor_max;
+		float param_mpc_jerk_auto;
+		float param_mpc_acc_up_max;
+		float param_mpc_acc_down_max;
+
+	} _params;
+
+	float _test_point_bearing{0.0f};
+	float _test_point_distance{0.0f};
+	float _vertical_test_point_distance{0.0f};
+	float _velocity_hor_abs{0.0f};
+	float _climbrate{0.0f};
+	float _current_alt_amsl{0.0f};
+	float _min_hor_dist_to_fence_mc{0.0f};
+	float _min_vert_dist_to_fence_mc{0.0f};
+
+	float _multirotor_braking_distance{0.0f};
+	float _multirotor_vertical_braking_distance{0.0f};
+
+	matrix::Vector2<double> _current_pos_lat_lon{};
+
+	void updateMinHorDistToFenceMultirotor();
+
+	void updateMinVertDistToFenceMultirotor();
+
+};

src/modules/navigator/geofence.cpp

@@ -58,7 +58,9 @@ Geofence::Geofence(Navigator *navigator) :
 	_sub_airdata(ORB_ID(vehicle_air_data))
 {
 	// we assume there's no concurrent fence update on startup
-	_updateFence();
+	if (_navigator != nullptr) {
+		_updateFence();
+	}
 }
 
 Geofence::~Geofence()
@@ -218,14 +220,13 @@ bool Geofence::check(const struct mission_item_s &mission_item)
 	return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude);
 }
 
-bool Geofence::checkAll(double lat, double lon, float altitude)
+bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
 {
 	bool inside_fence = true;
 
 	if (isHomeRequired() && _navigator->home_position_valid()) {
 
 		const float max_horizontal_distance = _param_gf_max_hor_dist.get();
-		const float max_vertical_distance = _param_gf_max_ver_dist.get();
 
 		const double home_lat = _navigator->get_home_position()->lat;
 		const double home_lon = _navigator->get_home_position()->lon;
@@ -236,20 +237,10 @@ bool Geofence::checkAll(double lat, double lon, float altitude)
 
 		get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z);
 
-		if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) {
-			if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home exceeded by %.1f m",
-						     (double)(dist_z - max_vertical_distance));
-				_last_vertical_range_warning = hrt_absolute_time();
-			}
-
-			inside_fence = false;
-		}
-
 		if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) {
 			if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home exceeded by %.1f m",
-						     (double)(dist_xy - max_horizontal_distance));
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)",
+						     (double)max_horizontal_distance);
 				_last_horizontal_range_warning = hrt_absolute_time();
 			}
 
@@ -257,9 +248,44 @@ bool Geofence::checkAll(double lat, double lon, float altitude)
 		}
 	}
 
+	return inside_fence;
+}
+
+bool Geofence::isBelowMaxAltitude(float altitude)
+{
+	bool inside_fence = true;
+
+	if (isHomeRequired() && _navigator->home_position_valid()) {
+
+		const float max_vertical_distance = _param_gf_max_ver_dist.get();
+		const float home_alt = _navigator->get_home_position()->alt;
+
+		float dist_z = altitude - home_alt;
+
+		if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) {
+			if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)",
+						     (double)max_vertical_distance);
+				_last_vertical_range_warning = hrt_absolute_time();
+			}
+
+			inside_fence = false;
+		}
+	}
+
+	return inside_fence;
+}
+
+
+bool Geofence::checkAll(double lat, double lon, float altitude)
+{
+	bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude);
+
+	inside_fence = inside_fence && isBelowMaxAltitude(altitude);
+
 	// to be inside the geofence both fences have to report being inside
 	// as they both report being inside when not enabled
-	inside_fence = inside_fence && checkPolygons(lat, lon, altitude);
+	inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude);
 
 	if (inside_fence) {
 		_outside_counter = 0;
@@ -277,8 +303,7 @@ bool Geofence::checkAll(double lat, double lon, float altitude)
 	}
 }
 
-
-bool Geofence::checkPolygons(double lat, double lon, float altitude)
+bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 {
 	// the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means
 	// the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now

src/modules/navigator/geofence.h

@@ -42,6 +42,7 @@
 
 #include <float.h>
 
+#include <lib/mathlib/mathlib.h>
 #include <px4_platform_common/module_params.h>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
@@ -62,7 +63,7 @@ public:
 	Geofence(Navigator *navigator);
 	Geofence(const Geofence &) = delete;
 	Geofence &operator=(const Geofence &) = delete;
-	~Geofence();
+	virtual ~Geofence();
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
 	enum {
@@ -97,6 +98,13 @@ public:
 	 */
 	bool check(const struct mission_item_s &mission_item);
 
+
+	bool isCloserThanMaxDistToHome(double lat, double lon, float altitude);
+
+	bool isBelowMaxAltitude(float altitude);
+
+	virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
+
 	int clearDm();
 
 	bool valid();
@@ -130,6 +138,14 @@ public:
 
 	bool isHomeRequired();
 
+	/**
+	 * Check if a point passes the Geofence test.
+	 * In addition to checkPolygons(), this takes all additional parameters into account.
+	 *
+	 * @return false for a geofence violation
+	 */
+	bool checkAll(double lat, double lon, float altitude);
+
 	/**
 	 * print Geofence status to the console
 	 */
@@ -188,13 +204,7 @@ private:
 	 */
 	bool checkPolygons(double lat, double lon, float altitude);
 
-	/**
-	 * Check if a point passes the Geofence test.
-	 * In addition to checkPolygons(), this takes all additional parameters into account.
-	 *
-	 * @return false for a geofence violation
-	 */
-	bool checkAll(double lat, double lon, float altitude);
+
 
 	bool checkAll(const vehicle_global_position_s &global_position);
 	bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl);