Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: restructure logic of adjust_velocity_circle_fence

This commit is contained in:
Randy Mackay 2019-06-07 18:30:52 +09:00
parent a96c146758
commit 607f996232
1 changed files with 48 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -289,6 +289,13 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
return;
}
// get desired speed
const float desired_speed = desired_vel_cms.length();
if (is_zero(desired_speed)) {
// no avoidance necessary when desired speed is zero
return;
}
const AP_AHRS &_ahrs = AP::ahrs();
// get position as a 2D offset from ahrs home
@ -299,49 +306,55 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
}
position_xy *= 100.0f; // m -> cm
const float speed = desired_vel_cms.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_cm = _fence.get_margin() * 100.0f;
// get vehicle distance from home
const float dist_from_home = position_xy.length();
if (!is_zero(speed) && (dist_from_home <= fence_radius)) {
// vehicle is inside the circular fence
if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) {
// implement sliding behaviour
const Vector2f stopping_point = position_xy + desired_vel_cms*(get_stopping_distance(kP, accel_cmss, speed)/speed);
const float stopping_point_dist_from_home = stopping_point.length();
if (stopping_point_dist_from_home > fence_radius - margin_cm) {
// unsafe desired velocity - will not be able to stop before reaching margin from fence
// Project stopping point radially onto fence boundary
// Adjusted velocity will point towards this projected point at a safe speed
const Vector2f target = stopping_point * ((fence_radius - margin_cm) / stopping_point_dist_from_home);
const Vector2f target_direction = target - position_xy;
const float distance_to_target = target_direction.length();
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
desired_vel_cms = target_direction * (MIN(speed,max_speed) / distance_to_target);
if (dist_from_home > fence_radius) {
// outside of circular fence, no velocity adjustments
return;
}
// vehicle is inside the circular fence
if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) {
// implement sliding behaviour
const Vector2f stopping_point = position_xy + desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed);
const float stopping_point_dist_from_home = stopping_point.length();
if (stopping_point_dist_from_home <= fence_radius - margin_cm) {
// stopping before before fence so no need to adjust
return;
}
// unsafe desired velocity - will not be able to stop before reaching margin from fence
// Project stopping point radially onto fence boundary
// Adjusted velocity will point towards this projected point at a safe speed
const Vector2f target_offset = stopping_point * ((fence_radius - margin_cm) / stopping_point_dist_from_home);
const Vector2f target_direction = target_offset - position_xy;
const float distance_to_target = target_direction.length();
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target);
} else {
// implement stopping behaviour
// calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
const Vector2f stopping_point_plus_margin = position_xy + desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed);
const float stopping_point_plus_margin_dist_from_home = stopping_point_plus_margin.length();
if (dist_from_home >= fence_radius - margin_cm) {
// if vehicle has already breached margin around fence
// if stopping point is even further from home (i.e. in wrong direction) then adjust speed to zero
// otherwise user is backing away from fence so do not apply limits
if (stopping_point_plus_margin_dist_from_home >= dist_from_home) {
desired_vel_cms.zero();
}
} else {
// implement stopping behaviour
const Vector2f stopping_point_plus_margin = position_xy + desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed);
const float stopping_point_plus_margin_dist_from_home = stopping_point_plus_margin.length();
if (dist_from_home >= fence_radius - margin_cm) {
// if vehicle has already breached margin around fence
// if stopping point is even further from home (i.e. in wrong direction) then adjust speed to zero
// otherwise user is backing away from fence so do not apply limits
if (stopping_point_plus_margin_dist_from_home >= dist_from_home) {
desired_vel_cms.zero();
}
} else {
// shorten vector without adjusting its direction
Vector2f intersection;
if (Vector2f::circle_segment_intersection(position_xy, stopping_point_plus_margin, Vector2f(0.0f,0.0f), fence_radius - margin_cm, intersection)) {
const float distance_to_target = MAX((intersection - position_xy).length() - margin_cm, 0.0f);
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
if (max_speed < speed) {
desired_vel_cms *= MAX(max_speed, 0.0f) / speed;
}
// shorten vector without adjusting its direction
Vector2f intersection;
if (Vector2f::circle_segment_intersection(position_xy, stopping_point_plus_margin, Vector2f(0.0f,0.0f), fence_radius - margin_cm, intersection)) {
const float distance_to_target = MAX((intersection - position_xy).length() - margin_cm, 0.0f);
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
if (max_speed < desired_speed) {
desired_vel_cms *= MAX(max_speed, 0.0f) / desired_speed;
}
}
}