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>

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();
@@ -137,7 +139,6 @@ 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();
 
@@ -157,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};
 
@@ -182,8 +186,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_MINDIST_GND>) _param_gf_min_dist_on_ground
+		(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
 	)
 
 	uORB::SubscriptionData<vehicle_air_data_s>	_sub_airdata;

src/modules/navigator/geofence_params.c

@@ -129,17 +129,3 @@ 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

@@ -424,8 +424,6 @@ 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

@@ -796,18 +796,9 @@ bool Navigator::geofence_breach_check_while_on_ground(bool &have_geofence_positi
 
 		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_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);
 
@@ -877,8 +868,6 @@ bool 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;