diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 45d7eea9b4..0ce460da03 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/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; + } +} diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index c383be2351..f7016a916c 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/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;