From 3fa15cec9148661aed009704b1c764a0c1b34ba0 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 10 Jul 2020 09:58:37 +0300 Subject: [PATCH] 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 --- .../geofence_breach_avoidance.cpp | 53 ++++++++++--------- .../geofence_breach_avoidance.h | 30 +++++++---- src/modules/navigator/geofence.h | 2 + src/modules/navigator/navigator_main.cpp | 8 ++- 4 files changed, 58 insertions(+), 35 deletions(-) diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index 0ab7606f4a..aab784811a 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/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 GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2 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); +} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h index 440911b0a1..5d5503f236 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -55,11 +55,11 @@ public: ~GeofenceBreachAvoidance(); + matrix::Vector2 getFenceViolationTestPoint(); + matrix::Vector2 waypointFromBearingAndDistance(matrix::Vector2 current_pos_lat_lon, float test_point_bearing, float test_point_distance); - matrix::Vector2 getFenceViolationTestPoint(); - matrix::Vector2 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 _current_pos_lat_lon{}; + matrix::Vector2 _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 waypointFromHomeToTestPointAtDist(float distance); + }; \ No newline at end of file diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 90d896cea1..34a5ff671f 100644 --- a/src/modules/navigator/geofence.h +++ b/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(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ef9435cfba..d02fd4a873 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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();