From 9d74d82ff6903fdc0c518d70e0a40790bb2f2b3a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 12 Dec 2017 21:19:37 +0900 Subject: [PATCH] AC_Avoid: add stopping behaviour --- libraries/AC_Avoidance/AC_Avoid.cpp | 87 ++++++++++++++++++++++------- libraries/AC_Avoidance/AC_Avoid.h | 7 +++ 2 files changed, 74 insertions(+), 20 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 84cffdb447..2b02cf5042 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -34,6 +34,13 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin, 2.0f), + // @Param: BEHAVE + // @DisplayName: Avoidance behaviour + // @Description: Avoidance behaviour (slide or stop) + // @Values: 0:Slide,1:Stop + // @User: Standard + AP_GROUPINFO("BEHAVE", 5, AC_Avoid, _behavior, 0), + AP_GROUPEND }; @@ -230,13 +237,25 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f float stopping_point_length = stopping_point.length(); if (stopping_point_length > fence_radius - margin_cm) { // Unsafe desired velocity - will not be able to stop before fence breach - // Project stopping point radially onto fence boundary - // Adjusted velocity will point towards this projected point at a safe speed - Vector2f target = stopping_point * ((fence_radius - margin_cm) / stopping_point_length); - Vector2f target_direction = target - position_xy; - float distance_to_target = target_direction.length(); - float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt); - desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target); + if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { + // Project stopping point radially onto fence boundary + // Adjusted velocity will point towards this projected point at a safe speed + const Vector2f target = stopping_point * ((fence_radius - margin_cm) / stopping_point_length); + const Vector2f target_direction = target - position_xy; + const float distance_to_target = target_direction.length(); + const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt); + desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target); + } else { + // shorten vector without adjusting its direction + Vector2f intersection; + if (Vector2f::circle_segment_intersection(position_xy, stopping_point, Vector2f(0.0f,0.0f), fence_radius, intersection)) { + const float distance_to_target = MAX((intersection - position_xy).length() - margin_cm, 0.0f); + const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt); + if (max_speed < speed) { + desired_vel *= MAX(max_speed, 0.0f) / speed; + } + } + } } } } @@ -354,26 +373,54 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des } // calc margin in cm - float margin_cm = MAX(margin * 100.0f, 0); + float margin_cm = MAX(margin * 100.0f, 0.0f); + + // for stopping + float speed = safe_vel.length(); + Vector2f stopping_point = position_xy + safe_vel*((2.0f + get_stopping_distance(kP, accel_cmss, speed))/speed); uint16_t i, j; for (i = 1, j = num_points-1; i < num_points; j = i++) { // end points of current edge Vector2f start = boundary[j]; Vector2f end = boundary[i]; - // vector from current position to closest point on current edge - Vector2f limit_direction = Vector2f::closest_point(position_xy, start, end) - position_xy; - // distance to closest point - const float limit_distance = limit_direction.length(); - if (!is_zero(limit_distance)) { - // We are strictly inside the given edge. - // Adjust velocity to not violate this edge. - limit_direction /= limit_distance; - limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - margin_cm,0.0f), dt); + if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { + // vector from current position to closest point on current edge + Vector2f limit_direction = Vector2f::closest_point(position_xy, start, end) - position_xy; + // distance to closest point + const float limit_distance = limit_direction.length(); + if (!is_zero(limit_distance)) { + // We are strictly inside the given edge. + // Adjust velocity to not violate this edge. + limit_direction /= limit_distance; + limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - margin_cm,0.0f), dt); + } else { + // We are exactly on the edge - treat this as a fence breach. + // i.e. do not adjust velocity. + return; + } } else { - // We are exactly on the edge - treat this as a fence breach. - // i.e. do not adjust velocity. - return; + // find intersection with line segment + Vector2f intersection; + if (Vector2f::segment_intersection(position_xy, stopping_point, start, end, intersection)) { + // vector from current position to point on current edge + Vector2f limit_direction = intersection - position_xy; + const float limit_distance = limit_direction.length(); + if (!is_zero(limit_distance)) { + if (limit_distance <= margin_cm) { + // we are within the margin so stop vehicle + safe_vel.zero(); + } else { + // vehicle inside the given edge, adjust velocity to not violate this edge + limit_direction /= limit_distance; + limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - margin_cm, 0.0f), dt); + } + } else { + // We are exactly on the edge - treat this as a fence breach. + // i.e. do not adjust velocity. + return; + } + } } } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index f1045d26a9..fe39ed9b2a 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -57,6 +57,12 @@ public: static const struct AP_Param::GroupInfo var_info[]; private: + // behaviour types (see BEHAVE parameter) + enum BehaviourType { + BEHAVIOR_SLIDE = 0, + BEHAVIOR_STOP = 1 + }; + /* * Adjusts the desired velocity for the circular fence. */ @@ -125,6 +131,7 @@ private: AP_Int16 _angle_max; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes) AP_Float _dist_max; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes + AP_Int8 _behavior; // avoidance behaviour (slide or stop) bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) };