GeofenceBreachAvoidance: improve behavior when using max dist to home option

- ensure that the vehicle is loitering with sufficient clearance to the boundary

Signed-off-by: Julian Kent <julian@auterion.com>

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp

@@ -68,37 +68,19 @@ void GeofenceBreachAvoidance::updateParameters()
 	updateMinHorDistToFenceMultirotor();
 }
 
-void GeofenceBreachAvoidance::setTestPointBearing(float test_point_bearing)
-{
-	_test_point_bearing = test_point_bearing;
-}
-
-void GeofenceBreachAvoidance::setTestPointDistance(float test_point_distance)
-{
-	_test_point_distance = test_point_distance;
-}
-
-void GeofenceBreachAvoidance::setVerticalTestPointDistance(float distance)
-{
-	_vertical_test_point_distance = distance;
-}
-
-void GeofenceBreachAvoidance::setHorizontalVelocity(float velocity_hor_abs)
-{
-	_velocity_hor_abs = velocity_hor_abs;
-}
-
-void GeofenceBreachAvoidance::setClimbRate(float climb_rate)
-{
-	_climbrate = climb_rate;
-}
-
 void GeofenceBreachAvoidance::setCurrentPosition(double lat, double lon, float alt)
 {
 	_current_pos_lat_lon = Vector2d(lat, lon);
 	_current_alt_amsl = alt;
 }
 
+void GeofenceBreachAvoidance::setHomePosition(double lat, double lon, float alt)
+{
+	_home_lat_lon(0) = lat;
+	_home_lat_lon(1) = lon;
+	_home_alt_amsl = alt;
+}
+
 matrix::Vector2<double> GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2<double>
 		current_pos_lat_lon, float test_point_bearing, float test_point_distance)
 {
@@ -160,6 +142,10 @@ GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type
 
 		return Vector2d(loiter_center_lat, loiter_center_lon);
 
+	} else if (violation_type.flags.dist_to_home_exceeded) {
+
+		return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - 2 * _test_point_distance, 0.0f));
+
 	} else {
 		return _current_pos_lat_lon;
 	}
@@ -198,6 +184,10 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ
 			return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance);
 		}
 
+	} else if (violation_type.flags.dist_to_home_exceeded) {
+
+		return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - _min_hor_dist_to_fence_mc, 0.0f));
+
 	} else {
 		if (_velocity_hor_abs > 0.5f) {
 			return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance);
@@ -291,3 +281,16 @@ void GeofenceBreachAvoidance::updateMinVertDistToFenceMultirotor()
 
 	_min_vert_dist_to_fence_mc =  2.0f * predictor.getCurrentPosition();
 }
+
+Vector2d GeofenceBreachAvoidance::waypointFromHomeToTestPointAtDist(float distance)
+{
+	Vector2d test_point = getFenceViolationTestPoint();
+	float bearing_home_current_pos = get_bearing_to_next_waypoint(_home_lat_lon(0), _home_lat_lon(1), test_point(0),
+					 test_point(1));
+	double loiter_center_lat, loiter_center_lon;
+
+	waypoint_from_heading_and_distance(_home_lat_lon(0), _home_lat_lon(1), bearing_home_current_pos, distance,
+					   &loiter_center_lat, &loiter_center_lon);
+
+	return Vector2d(loiter_center_lat, loiter_center_lon);
+}

src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h

@@ -55,11 +55,11 @@ public:
 
 	~GeofenceBreachAvoidance();
 
+	matrix::Vector2<double> getFenceViolationTestPoint();
+
 	matrix::Vector2<double> waypointFromBearingAndDistance(matrix::Vector2<double> current_pos_lat_lon,
 			float test_point_bearing, float test_point_distance);
 
-	matrix::Vector2<double> getFenceViolationTestPoint();
-
 	matrix::Vector2<double>
 	generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence);
 
@@ -73,24 +73,29 @@ public:
 
 	float generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type);
 
-	float getMinDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;}
+	float getMinHorDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;}
 
 	float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;}
 
-	void setTestPointBearing(float test_point_bearing);
+	void setTestPointBearing(float test_point_bearing) { _test_point_bearing = test_point_bearing; }
 
-	void setTestPointDistance(float test_point_distance);
+	void setHorizontalTestPointDistance(float test_point_distance) { _test_point_distance = test_point_distance; }
 
-	void setVerticalTestPointDistance(float distance);
+	void setVerticalTestPointDistance(float distance) { _vertical_test_point_distance = distance; }
 
-	void setHorizontalVelocity(float velocity_hor_abs);
+	void setHorizontalVelocity(float velocity_hor_abs) { _velocity_hor_abs = velocity_hor_abs; }
 
-	void setClimbRate(float climbrate);
+	void setClimbRate(float climbrate) { _climbrate = climbrate; }
 
 	void setCurrentPosition(double lat, double lon, float alt);
 
-	void updateParameters();
+	void setHomePosition(double lat, double lon, float alt);
 
+	void setMaxHorDistHome(float dist) { _max_hor_dist_home = dist; }
+
+	void setMaxVerDistHome(float dist) { _max_ver_dist_home = dist; }
+
+	void updateParameters();
 
 private:
 	struct {
@@ -126,9 +131,16 @@ private:
 	float _multirotor_vertical_braking_distance{0.0f};
 
 	matrix::Vector2<double> _current_pos_lat_lon{};
+	matrix::Vector2<double> _home_lat_lon {};
+	float _home_alt_amsl{0.0f};
+
+	float _max_hor_dist_home{0.0f};
+	float _max_ver_dist_home{0.0f};
 
 	void updateMinHorDistToFenceMultirotor();
 
 	void updateMinVertDistToFenceMultirotor();
 
+	matrix::Vector2<double> waypointFromHomeToTestPointAtDist(float distance);
+
 };

src/modules/navigator/geofence.h

@@ -135,6 +135,8 @@ public:
 	int getAltitudeMode() { return _param_gf_altmode.get(); }
 	int getSource() { return _param_gf_source.get(); }
 	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(); }
 
 	bool isHomeRequired();
 

src/modules/navigator/navigator_main.cpp

@@ -512,10 +512,16 @@ Navigator::run()
 				}
 			}
 
-			_gf_breach_avoidance.setTestPointDistance(test_point_distance);
+			_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
 			_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
 			_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
 			_gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
+			_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
+			_gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
+
+			if (home_position_valid()) {
+				_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
+			}
 
 			fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();