AC_Avoid: add support for stopping at polygon fence

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -33,6 +33,7 @@ void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector2f
 
     if (_enabled == AC_AVOID_STOP_AT_FENCE) {
         adjust_velocity_circle(kP, accel_cmss_limited, desired_vel);
+        adjust_velocity_poly(kP, accel_cmss_limited, desired_vel);
     }
 }
 
@@ -77,6 +78,86 @@ void AC_Avoid::adjust_velocity_circle(const float kP, const float accel_cmss, Ve
     }
 }
 
+/*
+ * Adjusts the desired velocity for the polygon fence.
+ */
+
+void AC_Avoid::adjust_velocity_poly(const float kP, const float accel_cmss, Vector2f &desired_vel)
+{
+    // exit if the polygon fence is not enabled
+    if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+        return;
+    }
+
+    // exit if the polygon fence has already been breached
+    if ((_fence.get_breaches() & AC_FENCE_TYPE_POLYGON) != 0) {
+        return;
+    }
+
+    // get polygon boundary
+    // Note: first point in list is the return-point (which copter does not use)
+    uint16_t num_points;
+    Vector2f* boundary = _fence.get_polygon_points(num_points);
+
+    // exit if there are no points
+    if (boundary == NULL || num_points == 0) {
+        return;
+    }
+
+    // do not adjust velocity if vehicle is outside the polygon fence
+    const Vector3f& position = _inav.get_position();
+    Vector2f position_xy(position.x, position.y);
+    if (_fence.boundary_breached(position_xy, num_points, boundary)) {
+        return;
+    }
+
+    // Safe_vel will be adjusted to remain within fence.
+    // We need a separate vector in case adjustment fails,
+    // e.g. if we are exactly on the boundary.
+    Vector2f safe_vel(desired_vel);
+
+    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 = 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, limit_distance);
+        } else {
+            // We are exactly on the edge - treat this as a fence breach.
+            // i.e. do not adjust velocity.
+            return;
+        }
+    }
+
+    desired_vel = safe_vel;
+}
+
+/*
+ * Limits the component of desired_vel in the direction of the unit vector
+ * limit_direction to be at most the maximum speed permitted by the limit_distance.
+ *
+ * Uses velocity adjustment idea from Randy's second email on this thread:
+ * https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
+ */
+void AC_Avoid::limit_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel, const Vector2f limit_direction, const float limit_distance) const
+{
+    const float max_speed = get_max_speed(kP, accel_cmss, limit_distance - get_margin());
+    // project onto limit direction
+    const float speed = desired_vel * limit_direction;
+    if (speed > max_speed) {
+        // subtract difference between desired speed and maximum acceptable speed
+        desired_vel += limit_direction*(max_speed - speed);
+    }
+}
+
 /*
  * Gets the current xy-position, relative to home (not relative to EKF origin)
  */
@@ -120,3 +201,31 @@ float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, co
         return linear_distance + (speed*speed)/(2.0f*accel_cmss);
     }
 }
+
+/*
+ * Returns the point closest to p on the line segment (v,w).
+ *
+ * Comments and implementation taken from
+ * http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
+ */
+Vector2f AC_Avoid::closest_point(Vector2f p, Vector2f v, Vector2f w) const
+{
+    // length squared of line segment
+    const float l2 = (v - w).length_squared();
+    if (is_zero(l2)) {
+        // v == w case
+        return v;
+    }
+    // Consider the line extending the segment, parameterized as v + t (w - v).
+    // We find projection of point p onto the line.
+    // It falls where t = [(p-v) . (w-v)] / |w-v|^2
+    // We clamp t from [0,1] to handle points outside the segment vw.
+    const float t = ((p - v) * (w - v)) / l2;
+    if (t <= 0) {
+        return v;
+    } else if (t >= 1) {
+        return w;
+    } else {
+        return v + (w - v)*t;
+    }
+}

libraries/AC_Avoidance/AC_Avoid.h

@@ -39,6 +39,21 @@ private:
      */
     void adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel);
 
+    /*
+     * Adjusts the desired velocity for the polygon fence.
+     */
+    void adjust_velocity_poly(const float kP, const float accel_cmss, Vector2f &desired_vel);
+
+
+    /*
+     * Limits the component of desired_vel in the direction of the unit vector
+     * limit_direction to be at most the maximum speed permitted by the limit_distance.
+     *
+     * Uses velocity adjustment idea from Randy's second email on this thread:
+     * https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
+     */
+    void limit_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel, const Vector2f limit_direction, const float limit_distance) const;
+
     /*
      * Gets the current position, relative to home (not relative to EKF origin)
      */
@@ -60,6 +75,11 @@ private:
      */
     float get_margin() const { return _fence.get_margin() * 100.0f; }
 
+    /*
+     * returns the point closest to p on the line segment (v,w)
+     */
+    Vector2f closest_point(Vector2f p, Vector2f v, Vector2f w) const;
+
     // external references
     const AP_AHRS& _ahrs;
     const AP_InertialNav& _inav;