geofence: added pre-arm check if too close to fence

- prevents dangerous situations during and after takeoff when the geofence
breach avoidance tries to push the vehicle away from the fence

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/geofence_result.msg

@@ -7,6 +7,7 @@ uint8 GF_ACTION_TERMINATE = 4               # flight termination
 uint8 GF_ACTION_LAND = 5		    # switch to AUTO|LAND
 
 bool geofence_violated		# true if the geofence is violated
+bool geofence_too_close_on_ground	# true if vehicle close than GF_MINDIST_GND while landed or disarmed
 uint8 geofence_action       	# action to take when geofence is violated
 
 bool home_required		# true if the geofence requires a valid home position

msg/vehicle_status.msg

@@ -88,6 +88,7 @@ bool high_latency_data_link_lost 			# Set to true if the high latency data link
 bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
 bool geofence_violated
+bool geofence_too_close_on_ground               # the vehicle is too close to a geofence boundary while being on the ground
 
 uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
 

src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -176,6 +176,14 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 		prearm_ok = false;
 	}
 
+	if (arm_requirements.geofence && status.geofence_too_close_on_ground) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle is too close to geofence");
+		}
+
+		prearm_ok = false;
+	}
+
 	// Arm Requirements: authorization
 	// check last, and only if everything else has passed
 	if (arm_requirements.arm_authorization && prearm_ok) {

src/modules/commander/Commander.cpp

@@ -2301,6 +2301,7 @@ Commander::run()
 		/* start geofence result check */
 		_geofence_result_sub.update(&_geofence_result);
 		_status.geofence_violated = _geofence_result.geofence_violated;
+		_status.geofence_too_close_on_ground = _geofence_result.geofence_too_close_on_ground;
 
 		const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW;
 

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h

@@ -46,6 +46,7 @@ union geofence_violation_type_u {
 		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
+		bool min_hor_dist_while_on_ground; ///< 3- too close to fence while on ground
 	} flags;
 	uint8_t value;
 };

src/modules/navigator/geofence.h

@@ -137,6 +137,7 @@ public:
 	int getGeofenceAction() { return _param_gf_action.get(); }
 	float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
 	float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
+	float getMinimumDistanceToFenceWhileOnGround() { return _param_gf_min_dist_on_ground.get(); }
 
 	bool isHomeRequired();
 
@@ -181,7 +182,8 @@ 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,
+		(ParamFloat<px4::params::GF_MINDIST_GND>) _param_gf_min_dist_on_ground
 	)
 
 	uORB::SubscriptionData<vehicle_air_data_s>	_sub_airdata;

src/modules/navigator/geofence_params.c

@@ -129,3 +129,17 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
  * @group Geofence
  */
 PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
+
+/**
+ * Minimum distance from fence when the vehicle is on the ground.
+ *
+ * If vehicle is closer than this distance while disarmed and landed, then 'geofence_too_close_on_ground' field
+ * in the geofence_result topic will be set to true.
+ *
+ * @unit m
+ * @min 5
+ * @max 1000
+ * @increment 1
+ * @group Geofence
+ */
+PARAM_DEFINE_FLOAT(GF_MINDIST_GND, 20);

src/modules/navigator/navigator.h

@@ -291,7 +291,9 @@ public:
 
 	bool		abort_landing();
 
-	void geofence_breach_check(bool &have_geofence_position_data);
+	bool geofence_breach_check(bool &have_geofence_position_data);
+
+	bool geofence_breach_check_while_on_ground(bool &have_geofence_position_data);
 
 	// Param access
 	float		get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
@@ -422,6 +424,8 @@ private:
 	bool _mission_landing_in_progress{false};	// this flag gets set if the mission is currently executing on a landing pattern
 	// if mission mode is inactive, this flag will be cleared after 2 seconds
 
+	float _fence_proximity_search_bearing{0.0f};	// bearing at which a fence proximity check is conducted while vehicle is on ground
+
 	traffic_buffer_s _traffic_buffer{};
 
 	// update subscriptions

src/modules/navigator/navigator_main.cpp

@@ -549,7 +549,21 @@ Navigator::run()
 		check_traffic();
 
 		/* Check geofence violation */
-		geofence_breach_check(have_geofence_position_data);
+		bool publish_geofence_result = false;
+
+		if (get_land_detected()->landed || !(_vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			publish_geofence_result = geofence_breach_check_while_on_ground(have_geofence_position_data);
+
+		} else {
+			publish_geofence_result = geofence_breach_check(have_geofence_position_data);
+		}
+
+		if (publish_geofence_result) {
+			_geofence_result.timestamp = hrt_absolute_time();
+			_geofence_result.geofence_action = _geofence.getGeofenceAction();
+			_geofence_result.home_required = _geofence.isHomeRequired();
+			_geofence_result_pub.publish(_geofence_result);
+		}
 
 		/* Do stuff according to navigation state set by commander */
 		NavigatorMode *navigation_mode_new{nullptr};
@@ -774,7 +788,37 @@ Navigator::run()
 	}
 }
 
-void Navigator::geofence_breach_check(bool &have_geofence_position_data)
+bool Navigator::geofence_breach_check_while_on_ground(bool &have_geofence_position_data)
+{
+	if (have_geofence_position_data &&
+	    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
+	    (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
+
+		have_geofence_position_data = false;
+
+		if (!_geofence_result.geofence_too_close_on_ground) {
+			// the last point did not violate the fence, let's try other directions
+			_fence_proximity_search_bearing = matrix::wrap_2pi(_fence_proximity_search_bearing + math::radians(30.0f));
+		}
+
+
+		double lat, lon;
+		waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, _fence_proximity_search_bearing,
+						   _geofence.getMinimumDistanceToFenceWhileOnGround(), &lat,
+						   &lon);
+
+		_geofence_result.geofence_too_close_on_ground = !_geofence.isInsidePolygonOrCircle(lat, lon, _global_pos.alt);
+		_geofence_result.geofence_violated =  !_geofence.isInsidePolygonOrCircle(_global_pos.lat, _global_pos.lon,
+						      _global_pos.alt);
+
+		return true;
+	}
+
+	return false;
+
+}
+
+bool Navigator::geofence_breach_check(bool &have_geofence_position_data)
 {
 
 	if (have_geofence_position_data &&
@@ -833,13 +877,11 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 				fence_violation_test_point(1),
 				_global_pos.alt);
 
+
+
 		_last_geofence_check = hrt_absolute_time();
 		have_geofence_position_data = false;
 
-		_geofence_result.timestamp = hrt_absolute_time();
-		_geofence_result.geofence_action = _geofence.getGeofenceAction();
-		_geofence_result.home_required = _geofence.isHomeRequired();
-
 		if (gf_violation_type.value) {
 			/* inform other apps via the mission result */
 			_geofence_result.geofence_violated = true;
@@ -902,8 +944,10 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 			_geofence_violation_warning_sent = false;
 		}
 
-		_geofence_result_pub.publish(_geofence_result);
+		return true;
 	}
+
+	return false;
 }
 
 int Navigator::task_spawn(int argc, char *argv[])