aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Avoid: restructure logic of adjust_velocity_circle_fence

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