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

AC_Avoid: stop includes margin for intersection calc 

the simpler stop feature (as opposed to slide) was not including the margin when looking for intersections with the boundary meaning it could slow too late
This commit is contained in:
Randy Mackay 2018-10-24 15:31:36 +09:00
parent 7fa592e673
commit b7f3140441
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -435,7 +435,7 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
// for stopping
float speed = safe_vel.length();
Vector2f stopping_point = position_xy + safe_vel*((2.0f + get_stopping_distance(kP, accel_cmss, speed))/speed);
Vector2f stopping_point_plus_margin = position_xy + safe_vel*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed);
uint16_t i, j;
for (i = 0, j = num_points-1; i < num_points; j = i++) {
@ -460,7 +460,7 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
} else {
// find intersection with line segment
Vector2f intersection;
if (Vector2f::segment_intersection(position_xy, stopping_point, start, end, intersection)) {
if (Vector2f::segment_intersection(position_xy, stopping_point_plus_margin, start, end, intersection)) {
// vector from current position to point on current edge
Vector2f limit_direction = intersection - position_xy;
const float limit_distance_cm = limit_direction.length();