2016-06-19 22:12:17 -03:00
|
|
|
#include "AC_Avoid.h"
|
|
|
|
|
|
|
|
const AP_Param::GroupInfo AC_Avoid::var_info[] = {
|
|
|
|
|
|
|
|
// @Param: ENABLE
|
|
|
|
// @DisplayName: Avoidance control enable/disable
|
|
|
|
// @Description: Enabled/disable stopping at fence
|
|
|
|
// @Values: 0:None,1:StopAtFence
|
|
|
|
// @User: Standard
|
|
|
|
AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_STOP_AT_FENCE),
|
|
|
|
|
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
|
|
|
/// Constructor
|
|
|
|
AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence)
|
|
|
|
: _ahrs(ahrs),
|
|
|
|
_inav(inav),
|
|
|
|
_fence(fence)
|
|
|
|
{
|
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel)
|
|
|
|
{
|
|
|
|
// exit immediately if disabled
|
|
|
|
if (_enabled == AC_AVOID_DISABLED) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// limit acceleration
|
|
|
|
float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
|
|
|
|
|
|
|
|
if (_enabled == AC_AVOID_STOP_AT_FENCE) {
|
|
|
|
adjust_velocity_circle(kP, accel_cmss_limited, desired_vel);
|
2016-06-22 23:45:01 -03:00
|
|
|
adjust_velocity_poly(kP, accel_cmss_limited, desired_vel);
|
2016-06-19 22:12:17 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Adjusts the desired velocity for the circular fence.
|
|
|
|
*/
|
|
|
|
void AC_Avoid::adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel)
|
|
|
|
{
|
|
|
|
// exit if circular fence is not enabled
|
|
|
|
if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-06-23 01:16:26 -03:00
|
|
|
// exit if the circular fence has already been breached
|
|
|
|
if ((_fence.get_breaches() & AC_FENCE_TYPE_CIRCLE) != 0) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-06-19 22:12:17 -03:00
|
|
|
// get position as a 2D offset in cm from ahrs home
|
|
|
|
const Vector2f position_xy = get_position();
|
|
|
|
|
|
|
|
float speed = desired_vel.length();
|
|
|
|
// get the fence radius in cm
|
|
|
|
const float fence_radius = _fence.get_radius() * 100.0f;
|
|
|
|
// get the margin to the fence in cm
|
|
|
|
const float margin = get_margin();
|
|
|
|
|
|
|
|
if (!is_zero(speed) && position_xy.length() <= fence_radius) {
|
|
|
|
// Currently inside circular fence
|
|
|
|
Vector2f stopping_point = position_xy + desired_vel*(get_stopping_distance(kP, accel_cmss, speed)/speed);
|
|
|
|
float stopping_point_length = stopping_point.length();
|
|
|
|
if (stopping_point_length > fence_radius - margin) {
|
|
|
|
// 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) / 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);
|
|
|
|
desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-06-22 23:45:01 -03:00
|
|
|
/*
|
|
|
|
* 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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-06-19 22:12:17 -03:00
|
|
|
/*
|
|
|
|
* Gets the current xy-position, relative to home (not relative to EKF origin)
|
|
|
|
*/
|
|
|
|
Vector2f AC_Avoid::get_position()
|
|
|
|
{
|
|
|
|
const Vector3f position_xyz = _inav.get_position();
|
|
|
|
const Vector2f position_xy(position_xyz.x,position_xyz.y);
|
|
|
|
const Vector2f diff = location_diff(_inav.get_origin(),_ahrs.get_home()) * 100.0f;
|
|
|
|
return position_xy - diff;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Computes the speed such that the stopping distance
|
|
|
|
* of the vehicle will be exactly the input distance.
|
|
|
|
*/
|
2016-06-23 01:14:01 -03:00
|
|
|
float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const float distance) const
|
2016-06-19 22:12:17 -03:00
|
|
|
{
|
|
|
|
return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss);
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Computes distance required to stop, given current speed.
|
|
|
|
*
|
|
|
|
* Implementation copied from AC_PosControl.
|
|
|
|
*/
|
2016-06-23 01:14:01 -03:00
|
|
|
float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, const float speed) const
|
2016-06-19 22:12:17 -03:00
|
|
|
{
|
|
|
|
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
|
|
|
|
if (kP <= 0.0f || accel_cmss <= 0.0f || is_zero(speed)) {
|
|
|
|
return 0.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
// calculate point at which velocity switches from linear to sqrt
|
|
|
|
float linear_speed = accel_cmss/kP;
|
|
|
|
|
|
|
|
// calculate distance within which we can stop
|
|
|
|
if (speed < linear_speed) {
|
|
|
|
return speed/kP;
|
|
|
|
} else {
|
|
|
|
float linear_distance = accel_cmss/(2.0f*kP*kP);
|
|
|
|
return linear_distance + (speed*speed)/(2.0f*accel_cmss);
|
|
|
|
}
|
|
|
|
}
|
2016-06-22 23:45:01 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
* 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;
|
|
|
|
}
|
|
|
|
}
|