fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 02:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

GeofenceBreachAvoidance: improve behavior when using max dist to home option

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

Signed-off-by: Julian Kent <julian@auterion.com>
This commit is contained in:
RomanBapst
2021-01-13 10:26:10 +01:00
committed by Julian Kent
parent a5dfa0c803
commit 3fa15cec91
4 changed files with 58 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
+28 -25
View File
@@ -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);
}