From 620f25503c48d264ede43a9a843106d2611fd3f1 Mon Sep 17 00:00:00 2001
From: benjinne <benjinne@gmail.com>
Date: Fri, 18 Mar 2022 06:44:13 -0400
Subject: [PATCH] Add geofence predict param (#17795)

---
 src/modules/navigator/geofence.h         |  4 +++-
 src/modules/navigator/geofence_params.c  | 12 ++++++++++++
 src/modules/navigator/navigator_main.cpp | 17 +++++++++++++----
 3 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index bd3542656b..9c99313986 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -144,6 +144,7 @@ public:
 
 	float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
 	float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
+	bool getPredict() { return _param_gf_predict.get(); }
 
 	bool isHomeRequired();
 
@@ -222,6 +223,7 @@ private:
 		(ParamInt<px4::params::GF_SOURCE>)         _param_gf_source,
 		(ParamInt<px4::params::GF_COUNT>)          _param_gf_count,
 		(ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
-		(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
+		(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist,
+		(ParamBool<px4::params::GF_PREDICT>)       _param_gf_predict
 	)
 };
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 42655d92e6..f8107e29b7 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -129,3 +129,15 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
  * @group Geofence
  */
 PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
+
+/**
+ * Use Pre-emptive geofence triggering
+ *
+ * Predict the motion of the vehicle and trigger the breach if it is determined that the current trajectory
+ * would result in a breach happening before the vehicle can make evasive maneuvers.
+ * The vehicle is then re-routed to a safe hold position (stop for multirotor, loiter for fixed wing).
+ *
+ * @boolean
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_PREDICT, 1);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3ee1620303..12577ef105 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -819,6 +819,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 		float test_point_bearing;
 		float test_point_distance;
 		float vertical_test_point_distance;
+		char geofence_violation_warning[50];
 
 		if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 			test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
@@ -851,7 +852,15 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 			_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
 		}
 
-		fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
+		if (_geofence.getPredict()) {
+			fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
+			snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Approaching on geofence");
+
+		} else {
+			fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
+			vertical_test_point_distance = 0;
+			snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Geofence exceeded");
+		}
 
 		gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
 				fence_violation_test_point(1),
@@ -877,9 +886,9 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 
 			/* Issue a warning about the geofence violation once and only if we are armed */
 			if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-				mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence\t");
-				events::send(events::ID("navigator_approach_geofence"), {events::Log::Warning, events::LogInternal::Info},
-					     "Approaching on Geofence");
+				mavlink_log_critical(&_mavlink_log_pub, "%s", geofence_violation_warning);
+				events::send(events::ID("navigator_geofence_violation"), {events::Log::Warning, events::LogInternal::Info},
+					     geofence_violation_warning);
 
 				// we have predicted a geofence violation and if the action is to loiter then
 				// demand a reposition to a location which is inside the geofence