From b7f3140441cdcf64da9044e1c32bdf7d1874b861 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Oct 2018 15:31:36 +0900 Subject: [PATCH] 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 --- libraries/AC_Avoidance/AC_Avoid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index e97412bebf..f6609d6ce2 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -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();