flight termination on geofence violation

3 files changed, 19 insertions, 0 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 04450a44f..522c6e886 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1244,6 +1244,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+
+			/* Check for geofence violation */
+			if (pos_sp_triplet.geofence_violated) {
+				//XXX: make this configurable to select different actions (e.g. navigation modes)
+				/* this will only trigger if geofence is activated via param and a geofence file is present */
+				armed.force_failsafe = true;
+				status_changed = true;
+			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 331a9a728..ba46bd568 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -335,6 +335,11 @@ Navigator::task_main()
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
+				/* inform other apps via the sp triplet */
+				_pos_sp_triplet.geofence_violated = true;
+				if (_pos_sp_triplet.geofence_violated != true) {
+					_pos_sp_triplet_updated = true;
+				}
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -342,6 +347,11 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
+				/* inform other apps via the sp triplet */
+				_pos_sp_triplet.geofence_violated = false;
+				if (_pos_sp_triplet.geofence_violated != false) {
+					_pos_sp_triplet_updated = true;
+				}
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4a1932180..4e8c6c53e 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -97,6 +97,7 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s next;
 
 	unsigned nav_state;				/**< report the navigation state */
+	bool geofence_violated;				/**< true if the geofence is violated */
 };
 
 /**