#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,2:UseProximitySensor,3:All // @Bitmask: 0:StopAtFence,1:UseProximitySensor // @User: Standard AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_ALL), AP_GROUPEND }; /// Constructor AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity) : _ahrs(ahrs), _inav(inav), _fence(fence), _proximity(proximity) { 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) > 0) { adjust_velocity_circle(kP, accel_cmss_limited, desired_vel); adjust_velocity_poly(kP, accel_cmss_limited, desired_vel); } if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0) { adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel); } } // convenience function to accept Vector3f. Only x and y are adjusted void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector3f &desired_vel) { Vector2f des_vel_xy(desired_vel.x, desired_vel.y); adjust_velocity(kP, accel_cmss, des_vel_xy); desired_vel.x = des_vel_xy.x; desired_vel.y = des_vel_xy.y; } /* * 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; } // exit if the circular fence has already been breached if ((_fence.get_breaches() & AC_FENCE_TYPE_CIRCLE) != 0) { return; } // 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); } } } /* * 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 == nullptr || 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; } /* * Adjusts the desired velocity based on output from the proximity sensor */ void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel) { // exit immediately if proximity sensor is not present if (_proximity.get_status() != AP_Proximity::Proximity_Good) { return; } // exit immediately if no desired velocity if (desired_vel.is_zero()) { return; } // normalise desired velocity vector Vector2f vel_dir = desired_vel.normalized(); // get angle of desired velocity float heading_rad = atan2f(vel_dir.y, vel_dir.x); // rotate desired velocity angle into body-frame angle float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw); // get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame) float distance_m; if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) { limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f)); } } /* * 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); // 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) */ 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. */ float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const float distance) const { return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss); } /* * Computes distance required to stop, given current speed. * * Implementation copied from AC_PosControl. */ float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, const float speed) const { // 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 distance within which we can stop // accel_cmss/kP is the point at which velocity switches from linear to sqrt if (speed < accel_cmss/kP) { return speed/kP; } else { // accel_cmss/(2.0f*kP*kP) is the distance at which we switch from linear to sqrt response return accel_cmss/(2.0f*kP*kP) + (speed*speed)/(2.0f*accel_cmss); } }