AC_Avoid: add stopping behaviour

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
+            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
-            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);
+                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,13 +373,18 @@ 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];
+        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
@@ -375,6 +399,29 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
                 // i.e. do not adjust velocity.
                 return;
             }
+        } else {
+            // 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;
+                }
+            }
+        }
     }
 
     // set modified desired velocity vector

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)
 };