/*
* 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));
}
}
/*
* Adjusts the desired velocity for the polygon fence.
*/
void AC_Avoid::adjust_velocity_polygon_fence(float kP, 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;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
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);
// adjust velocity using polygon
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true);
}
void Plane::calc_gndspeed_undershoot()
{
// Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
Vector2f gndVel = ahrs.groundspeed_vector();
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
if (!yawVect.is_zero()) {
yawVect.normalize();
float gndSpdFwd = yawVect * gndVel;
groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0;
}
} else {
groundspeed_undershoot = 0;
}
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(float kP, 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;
}
// get boundary from proximity sensor
uint16_t num_points;
const Vector2f *boundary = _proximity.get_boundary_points(num_points);
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, false);
}
// find next boundary point from an array of boundary points given the current index into that array
// returns true if a next point can be found
// current_index should be an index into the boundary_pts array
// start_angle is an angle (in radians), the search will sweep clockwise from this angle
// the index of the next point is returned in the next_index argument
// the angle to the next point is returned in the next_angle argument
bool AP_Beacon::get_next_boundary_point(const Vector2f* boundary_pts, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t& next_index, float& next_angle)
{
// sanity check
if (boundary_pts == nullptr || current_index >= num_points) {
return false;
}
// get current point
Vector2f curr_point = boundary_pts[current_index];
// search through all points for next boundary point in a clockwise direction
float lowest_angle = M_PI_2;
float lowest_angle_relative = M_PI_2;
bool lowest_found = false;
uint8_t lowest_index = 0;
for (uint8_t i=0; i < num_points; i++) {
if (i != current_index) {
Vector2f vec = boundary_pts[i] - curr_point;
if (!vec.is_zero()) {
float angle = wrap_2PI(atan2f(vec.y, vec.x));
float angle_relative = wrap_2PI(angle - start_angle);
if ((angle_relative < lowest_angle_relative) || !lowest_found) {
lowest_angle = angle;
lowest_angle_relative = angle_relative;
lowest_index = i;
lowest_found = true;
}
}
}
}
// return results
if (lowest_found) {
next_index = lowest_index;
next_angle = lowest_angle;
}
return lowest_found;
}
