From 4c717e2714b982ef41b1f43846a27e4273463f2f Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 9 Dec 2021 11:30:10 +0300 Subject: [PATCH] geofence: added pre-arm check if too close to fence - prevents dangerous situations during and after takeoff when the geofence breach avoidance tries to push the vehicle away from the fence Signed-off-by: RomanBapst --- msg/geofence_result.msg | 1 + msg/vehicle_status.msg | 1 + .../PreFlightCheck/checks/preArmCheck.cpp | 8 +++ src/modules/commander/Commander.cpp | 1 + .../geofence_breach_avoidance.h | 1 + src/modules/navigator/geofence.h | 4 +- src/modules/navigator/geofence_params.c | 14 +++++ src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 58 ++++++++++++++++--- 9 files changed, 85 insertions(+), 9 deletions(-) diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index fcae7a28c4..51051ab7fd 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -7,6 +7,7 @@ uint8 GF_ACTION_TERMINATE = 4 # flight termination uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND bool geofence_violated # true if the geofence is violated +bool geofence_too_close_on_ground # true if vehicle close than GF_MINDIST_GND while landed or disarmed uint8 geofence_action # action to take when geofence is violated bool home_required # true if the geofence requires a valid home position diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 8c9d7e2d1d..539435cd0d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -88,6 +88,7 @@ bool high_latency_data_link_lost # Set to true if the high latency data link bool engine_failure # Set to true if an engine failure is detected bool mission_failure # Set to true if mission could not continue/finish bool geofence_violated +bool geofence_too_close_on_ground # the vehicle is too close to a geofence boundary while being on the ground uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL] diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index d0d113ece8..c18c69121b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -176,6 +176,14 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st prearm_ok = false; } + if (arm_requirements.geofence && status.geofence_too_close_on_ground) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle is too close to geofence"); + } + + prearm_ok = false; + } + // Arm Requirements: authorization // check last, and only if everything else has passed if (arm_requirements.arm_authorization && prearm_ok) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 79a1f3defd..a0f150c0ae 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2301,6 +2301,7 @@ Commander::run() /* start geofence result check */ _geofence_result_sub.update(&_geofence_result); _status.geofence_violated = _geofence_result.geofence_violated; + _status.geofence_too_close_on_ground = _geofence_result.geofence_too_close_on_ground; const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h index 1a6c63ea20..c49260ff8d 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -46,6 +46,7 @@ union geofence_violation_type_u { bool dist_to_home_exceeded: 1; ///< 0 - distance to home exceeded bool max_altitude_exceeded: 1; ///< 1 - maximum altitude exceeded bool fence_violation: 1; ///< 2- violation of user defined fence + bool min_hor_dist_while_on_ground; ///< 3- too close to fence while on ground } flags; uint8_t value; }; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 7ab0cbcbf0..1737515232 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -137,6 +137,7 @@ 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(); @@ -181,7 +182,8 @@ private: (ParamInt) _param_gf_source, (ParamInt) _param_gf_count, (ParamFloat) _param_gf_max_hor_dist, - (ParamFloat) _param_gf_max_ver_dist + (ParamFloat) _param_gf_max_ver_dist, + (ParamFloat) _param_gf_min_dist_on_ground ) uORB::SubscriptionData _sub_airdata; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 42655d92e6..a98ce5e349 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -129,3 +129,17 @@ 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 f585275b24..c6489f2010 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -291,7 +291,9 @@ public: bool abort_landing(); - void geofence_breach_check(bool &have_geofence_position_data); + bool geofence_breach_check(bool &have_geofence_position_data); + + bool geofence_breach_check_while_on_ground(bool &have_geofence_position_data); // Param access float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } @@ -422,6 +424,8 @@ 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 7457b83a14..ec31ff4688 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -549,7 +549,21 @@ Navigator::run() check_traffic(); /* Check geofence violation */ - geofence_breach_check(have_geofence_position_data); + bool publish_geofence_result = false; + + if (get_land_detected()->landed || !(_vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + publish_geofence_result = geofence_breach_check_while_on_ground(have_geofence_position_data); + + } else { + publish_geofence_result = geofence_breach_check(have_geofence_position_data); + } + + if (publish_geofence_result) { + _geofence_result.timestamp = hrt_absolute_time(); + _geofence_result.geofence_action = _geofence.getGeofenceAction(); + _geofence_result.home_required = _geofence.isHomeRequired(); + _geofence_result_pub.publish(_geofence_result); + } /* Do stuff according to navigation state set by commander */ NavigatorMode *navigation_mode_new{nullptr}; @@ -774,7 +788,37 @@ Navigator::run() } } -void Navigator::geofence_breach_check(bool &have_geofence_position_data) +bool Navigator::geofence_breach_check_while_on_ground(bool &have_geofence_position_data) +{ + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) { + + 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_violated = !_geofence.isInsidePolygonOrCircle(_global_pos.lat, _global_pos.lon, + _global_pos.alt); + + return true; + } + + return false; + +} + +bool Navigator::geofence_breach_check(bool &have_geofence_position_data) { if (have_geofence_position_data && @@ -833,13 +877,11 @@ void 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; - _geofence_result.timestamp = hrt_absolute_time(); - _geofence_result.geofence_action = _geofence.getGeofenceAction(); - _geofence_result.home_required = _geofence.isHomeRequired(); - if (gf_violation_type.value) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; @@ -902,8 +944,10 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) _geofence_violation_warning_sent = false; } - _geofence_result_pub.publish(_geofence_result); + return true; } + + return false; } int Navigator::task_spawn(int argc, char *argv[])