/* * 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; }