From b7b03c1b5e40c21a30fc172e9ea7cde95f05c9c1 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 3 Jan 2022 16:46:12 +0300 Subject: [PATCH] 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 --- src/modules/navigator/geofence.cpp | 20 ++++++++++++++++++++ src/modules/navigator/geofence.h | 9 ++++++--- src/modules/navigator/geofence_params.c | 14 -------------- src/modules/navigator/navigator.h | 2 -- src/modules/navigator/navigator_main.cpp | 17 +++-------------- 5 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9b92b3d0dd..50ae53a14f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/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() { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 1737515232..445b733c7d 100644 --- a/src/modules/navigator/geofence.h +++ b/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) _param_gf_source, (ParamInt) _param_gf_count, (ParamFloat) _param_gf_max_hor_dist, - (ParamFloat) _param_gf_max_ver_dist, - (ParamFloat) _param_gf_min_dist_on_ground + (ParamFloat) _param_gf_max_ver_dist ) uORB::SubscriptionData _sub_airdata; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index a98ce5e349..42655d92e6 100644 --- a/src/modules/navigator/geofence_params.c +++ b/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); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index c6489f2010..2249e30746 100644 --- a/src/modules/navigator/navigator.h +++ b/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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ec31ff4688..a7862202ef 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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;