geofence violation actions

msg/geofence_result.msg

@@ -1 +1,8 @@
+uint8 GF_ACTION_NONE = 0                    # no action on geofence violation
+uint8 GF_ACTION_WARN = 1                    # critical mavlink message
+uint8 GF_ACTION_LOITER = 2                  # switch to AUTO|LOITER
+uint8 GF_ACTION_RTL = 3                     # switch to AUTO|RTL
+uint8 GF_ACTION_TERMINATE = 4               # flight termination
+
 bool geofence_violated		# true if the geofence is violated
+uint8 geofence_action       # action to take when geofence is violated

src/modules/commander/commander.cpp

@@ -195,13 +195,7 @@ static struct offboard_control_mode_s offboard_control_mode;
 static struct home_position_s _home;
 
 static unsigned _last_mission_instance = 0;
-static uint64_t last_manual_input = 0;
-static switch_pos_t last_offboard_switch = 0;
-static switch_pos_t last_return_switch = 0;
-static switch_pos_t last_mode_switch = 0;
-static switch_pos_t last_acro_switch = 0;
-static switch_pos_t last_posctl_switch = 0;
-static switch_pos_t last_loiter_switch = 0;
+static manual_control_setpoint_s _last_sp_man;
 
 struct vtol_vehicle_status_s vtol_status;
 
@@ -922,6 +916,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
 	param_t _param_eph = param_find("COM_HOME_H_T");
 	param_t _param_epv = param_find("COM_HOME_V_T");
+	param_t _param_geofence_action = param_find("GF_ACTION");
 
 	// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
 	// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
@@ -1260,6 +1255,8 @@ int commander_thread_main(int argc, char *argv[])
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
 
+	uint8_t geofence_action = 0;
+
 	/* Thresholds for engine failure detection */
 	int32_t ef_throttle_thres = 1.0f;
 	int32_t ef_current2throttle_thres = 0.0f;
@@ -1346,6 +1343,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
+			param_get(_param_geofence_action, &geofence_action);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1835,24 +1833,106 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
 		}
 
-		/* Check for geofence violation */
-		if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
-			//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, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+		// Geofence actions
+		if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) {
+
+			static bool geofence_loiter_on = false;
+			static bool geofence_rtl_on = false;
+
+			static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX;
+
+			// check for geofence violation
+			if (geofence_result.geofence_violated) {
+				static hrt_abstime last_geofence_violation = 0;
+				const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds
+				if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) {
+
+					last_geofence_violation = hrt_absolute_time();
+
+					switch (geofence_result.geofence_action) {
+						case (geofence_result_s::GF_ACTION_NONE) : {
+							// do nothing
+							break;
+						}
+						case (geofence_result_s::GF_ACTION_WARN) : {
+							// do nothing, mavlink critical messages are sent by navigator
+							break;
+						}
+						case (geofence_result_s::GF_ACTION_LOITER) : {
+							if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) {
+								geofence_loiter_on = true;
+							}
+							break;
+						}
+						case (geofence_result_s::GF_ACTION_RTL) : {
+							if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) {
+								geofence_rtl_on = true;
+							}
+							break;
+						}
+						case (geofence_result_s::GF_ACTION_TERMINATE) : {
+							warnx("Flight termination because of geofence");
+							mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
+							armed.force_failsafe = true;
+							status_changed = true;
+							break;
+						}
+					}
+				}
+			}
+
+			// reset if no longer in LOITER or if manually switched to LOITER
+			geofence_loiter_on = geofence_loiter_on
+									&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER)
+									&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
+
+			// reset if no longer in RTL or if manually switched to RTL
+			geofence_rtl_on = geofence_rtl_on
+								&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL)
+								&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
+
+			if (!geofence_loiter_on && !geofence_rtl_on) {
+				// store the last good main_state when not in a geofence triggered action (LOITER or RTL)
+				geofence_main_state_before_violation = status.main_state;
+			}
+
+			// revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST
+			if ((geofence_loiter_on || geofence_rtl_on) &&
+			   (geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL ||
+				geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL ||
+				geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL ||
+				geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO ||
+				geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) {
+
+				// transition to previous state if sticks are increased
+				const float min_stick_change = 0.2;
+				if ((_last_sp_man.timestamp != sp_man.timestamp) &&
+					((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
+					 (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
+					 (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
+					 (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
+
+					main_state_transition(&status, geofence_main_state_before_violation);
+				}
+			}
+		}
+
+
+		/* Check for mission flight termination */
+		if (armed.armed && mission_result.flight_termination) {
 			armed.force_failsafe = true;
 			status_changed = true;
 			static bool flight_termination_printed = false;
 
 			if (!flight_termination_printed) {
-				warnx("Flight termination because of navigator request or geofence");
-				mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
+				warnx("Flight termination because of navigator request");
 				flight_termination_printed = true;
 			}
 
 			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 				mavlink_log_critical(mavlink_fd, "Flight termination active");
 			}
-		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+		}
 
 		/* Only evaluate mission state if home is set,
 		 * this prevents false positives for the mission
@@ -1937,10 +2017,15 @@ int commander_thread_main(int argc, char *argv[])
 					 * for being in manual mode only applies to manual arming actions.
 					 * the system can be armed in auto if armed via the GCS.
 					 */
+
 					if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
 						(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
+					} else if (!status.condition_home_position_valid &&
+								geofence_action == geofence_result_s::GF_ACTION_RTL) {
+						print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
+
 					} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 						arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
 										     mavlink_fd);
@@ -2107,7 +2192,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* Check for failure combinations which lead to flight termination */
 		if (armed.armed) {
 			/* At this point the data link and the gps system have been checked
-			 * If  we are not in a manual (RC stick controlled mode)
+			 * If we are not in a manual (RC stick controlled mode)
 			 * and both failed we want to terminate the flight */
 			if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
@@ -2422,31 +2507,32 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
-	/* if offboard is set allready by a mavlink command, abort */
+	/* if offboard is set already by a mavlink command, abort */
 	if (status.offboard_control_set_by_command) {
 		return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 	}
 
 	/* manual setpoint has not updated, do not re-evaluate it */
-	if ((last_manual_input == sp_man->timestamp) ||
-		((last_offboard_switch == sp_man->offboard_switch) &&
-		 (last_return_switch == sp_man->return_switch) &&
-		 (last_mode_switch == sp_man->mode_switch) &&
-		 (last_acro_switch == sp_man->acro_switch) &&
-		 (last_posctl_switch == sp_man->posctl_switch) &&
-		 (last_loiter_switch == sp_man->loiter_switch))) {
+	if ((_last_sp_man.timestamp == sp_man->timestamp) ||
+		((_last_sp_man.offboard_switch == sp_man->offboard_switch) &&
+		 (_last_sp_man.return_switch == sp_man->return_switch) &&
+		 (_last_sp_man.mode_switch == sp_man->mode_switch) &&
+		 (_last_sp_man.acro_switch == sp_man->acro_switch) &&
+		 (_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
+		 (_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
+
+		// update these fields for the geofence system
+		_last_sp_man.timestamp = sp_man->timestamp;
+		_last_sp_man.x = sp_man->x;
+		_last_sp_man.y = sp_man->y;
+		_last_sp_man.z = sp_man->z;
+		_last_sp_man.r = sp_man->r;
 
 		/* no timestamp change or no switch change -> nothing changed */
 		return TRANSITION_NOT_CHANGED;
 	}
 
-	last_manual_input = sp_man->timestamp;
-	last_offboard_switch = sp_man->offboard_switch;
-	last_return_switch = sp_man->return_switch;
-	last_mode_switch = sp_man->mode_switch;
-	last_acro_switch = sp_man->acro_switch;
-	last_posctl_switch = sp_man->posctl_switch;
-	last_loiter_switch = sp_man->loiter_switch;
+	_last_sp_man = *sp_man;
 
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {

src/modules/navigator/geofence.cpp

@@ -53,14 +53,8 @@
 #include <geo/geo.h>
 #include <drivers/drv_hrt.h>
 
-#define GEOFENCE_OFF 0
-#define GEOFENCE_FILE_ONLY 1
-#define GEOFENCE_MAX_DISTANCES_ONLY 2
-#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
-
 #define GEOFENCE_RANGE_WARNING_LIMIT 3000000
 
-
 /* Oddly, ERROR is not defined for C++ */
 #ifdef ERROR
 # undef ERROR
@@ -76,8 +70,8 @@ Geofence::Geofence() :
 	_last_vertical_range_warning(0),
 	_altitude_min(0),
 	_altitude_max(0),
-	_verticesCount(0),
-	_param_geofence_mode(this, "MODE"),
+	_vertices_count(0),
+	_param_action(this, "ACTION"),
 	_param_altitude_mode(this, "ALTMODE"),
 	_param_source(this, "SOURCE"),
 	_param_counter_threshold(this, "COUNT"),
@@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
-	if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
 		int32_t max_horizontal_distance = _param_max_hor_distance.get();
 		int32_t max_vertical_distance = _param_max_ver_distance.get();
 
@@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 				if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
 					if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-						mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
+						mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
 								     (double)(dist_z - max_vertical_distance));
 						_last_vertical_range_warning = hrt_absolute_time();
 					}
@@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 				if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
 					if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-						mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
+						mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
 								     (double)(dist_xy - max_horizontal_distance));
 						_last_horizontal_range_warning = hrt_absolute_time();
 					}
@@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
 				}
 			}
 		}
-	}
 
 	bool inside_fence = inside_polygon(lat, lon, altitude);
 
@@ -194,12 +186,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
-	/* Return true if geofence is disabled or only checking max distances */
-	if ((_param_geofence_mode.get() == GEOFENCE_OFF)
-	    || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
-		return true;
-	}
-
 	if (valid()) {
 
 		if (!isEmpty()) {
@@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude)
 			struct fence_vertex_s temp_vertex_j;
 
 			/* Red until fence is finished */
-			for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+			for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) {
 				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
 					break;
 				}
@@ -259,7 +245,7 @@ Geofence::valid()
 	}
 
 	// Otherwise
-	if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) {
+	if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) {
 		warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1);
 		return false;
 	}
@@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename)
 
 	/* Check if import was successful */
 	if (gotVertical && pointCounter > 0) {
-		_verticesCount = pointCounter;
+		_vertices_count = pointCounter;
 		warnx("Geofence: imported successfully");
 		mavlink_log_info(_mavlinkFd, "Geofence imported");
 		rc = OK;

src/modules/navigator/geofence.h

@@ -97,12 +97,14 @@ public:
 
 	int loadFromFile(const char *filename);
 
-	bool isEmpty() {return _verticesCount == 0;}
+	bool isEmpty() {return _vertices_count == 0;}
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
 
 	int getSource() { return _param_source.get(); }
 
+	int getGeofenceAction() { return _param_action.get(); }
+
 	void setMavlinkFd(int value) { _mavlinkFd = value; }
 
 private:
@@ -114,20 +116,20 @@ private:
 	hrt_abstime _last_horizontal_range_warning;
 	hrt_abstime _last_vertical_range_warning;
 
-	float			_altitude_min;
-	float			_altitude_max;
+	float _altitude_min;
+	float _altitude_max;
 
-	unsigned 			_verticesCount;
+	uint8_t _vertices_count;
 
 	/* Params */
-	control::BlockParamInt _param_geofence_mode;
+	control::BlockParamInt _param_action;
 	control::BlockParamInt _param_altitude_mode;
 	control::BlockParamInt _param_source;
 	control::BlockParamInt _param_counter_threshold;
 	control::BlockParamInt _param_max_hor_distance;
 	control::BlockParamInt _param_max_ver_distance;
 
-	uint8_t			_outside_counter;
+	uint8_t	_outside_counter;
 
 	int _mavlinkFd;
 

src/modules/navigator/geofence_params.c

@@ -44,15 +44,15 @@
  */
 
 /**
- * Geofence mode.
+ * Geofence violation action.
  *
- * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both
+ * 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination
  *
  * @min 0
- * @max 3
+ * @max 4
  * @group Geofence
  */
-PARAM_DEFINE_INT32(GF_MODE, 0);
+PARAM_DEFINE_INT32(GF_ACTION, 1);
 
 /**
  * Geofence altitude mode
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1);
 /**
  * Max horizontal distance in meters.
  *
- * Set to > 0 to activate RTL if horizontal distance to home exceeds this value.
+ * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value.
  *
  * @group Geofence
  */
@@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1);
 /**
  * Max vertical distance in meters.
  *
- * Set to > 0 to activate RTL if vertical distance to home exceeds this value.
+ * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value.
  *
  * @group Geofence
  */

src/modules/navigator/navigator_main.cpp

@@ -405,10 +405,14 @@ Navigator::task_main()
 
 		/* Check geofence violation */
 		static hrt_abstime last_geofence_check = 0;
-		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
+		if (have_geofence_position_data &&
+			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
+			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
 			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
+
+			_geofence_result.geofence_action = _geofence.getGeofenceAction();
 			if (!inside) {
 				/* inform other apps via the mission result */
 				_geofence_result.geofence_violated = true;