addressed review comments: added a geofence method which tells if vehicle

is closer to the fence than the minimum distance (hardcoded)

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.cpp

@@ -458,6 +458,26 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
 	return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius;
 }
 
+bool Geofence::isCloserThanMinDistToFenceWhenLanded(const double lat, const double lon, const float alt)
+{
+	float test_bearing = 0.0f;
+	double test_point_lat, test_point_lon;
+
+	while (test_bearing < M_TWOPI_F) {
+		waypoint_from_heading_and_distance(lat, lon, test_bearing,
+						   _min_dist_to_fence_when_landed, &test_point_lat,
+						   &test_point_lon);
+
+		if (!isInsidePolygonOrCircle(test_point_lat, test_point_lon, alt)) {
+			return true;
+		}
+
+		test_bearing += math::radians(30.0f);
+	}
+
+	return false;
+}
+
 bool
 Geofence::valid()
 {

src/modules/navigator/geofence.h

@@ -105,6 +105,8 @@ public:
 
 	virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
 
+	bool isCloserThanMinDistToFenceWhenLanded(const double lat, const double lon, const float alt);
+
 	int clearDm();
 
 	bool valid();
@@ -156,6 +158,9 @@ public:
 private:
 	Navigator	*_navigator{nullptr};
 
+	static constexpr float _min_dist_to_fence_when_landed =
+		20.0f;	// if exceeded will cause 'geofence_too_close_on_ground' flag in 'geofence_result' to trigger
+
 	hrt_abstime _last_horizontal_range_warning{0};
 	hrt_abstime _last_vertical_range_warning{0};
 

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(); }

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,28 @@ 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;
+
+		_geofence_result.geofence_too_close_on_ground = _geofence.isCloserThanMinDistToFenceWhenLanded(_global_pos.lat,
+				_global_pos.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 &&
@@ -836,10 +871,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 		_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 +933,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[])