/*
* Adjusts the desired velocity for the polygon fence.
*/
void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame)
{
// exit if there are no points
if (boundary == nullptr || num_points == 0) {
return;
}
// do not adjust velocity if vehicle is outside the polygon fence
Vector3f position;
if (earth_frame) {
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);
// if boundary points are in body-frame, rotate velocity vector from earth frame to body-frame
if (!earth_frame) {
safe_vel.x = desired_vel.y * _ahrs.sin_yaw() + desired_vel.x * _ahrs.cos_yaw(); // right
safe_vel.y = desired_vel.y * _ahrs.cos_yaw() - desired_vel.x * _ahrs.sin_yaw(); // forward
}
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 - get_margin(),0.0f));
} 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
if (earth_frame) {
desired_vel = safe_vel;
} else {
// if points were in body-frame, rotate resulting vector back to earth-frame
desired_vel.x = safe_vel.x * _ahrs.cos_yaw() - safe_vel.y * _ahrs.sin_yaw();
desired_vel.y = safe_vel.x * _ahrs.sin_yaw() + safe_vel.y * _ahrs.cos_yaw();
}
}
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 = 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 - get_margin(),0.0f));
} else {
// We are exactly on the edge - treat this as a fence breach.
// i.e. do not adjust velocity.
return;
}
}
desired_vel = safe_vel;
}
