Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Circle: use stopping point to get closest point on circle

Browse Source
This commit is contained in:
liang.tang 2018-07-20 14:08:41 +08:00 committed by Randy Mackay
parent 1aeb9446c0
commit 8664ed006a
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -176,12 +176,13 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result)
}
// get current position
const Vector3f &curr_pos = _inav.get_position();
Vector3f stopping_point;
_pos_control.get_stopping_point_xy(stopping_point);
// calc vector from current location to circle center
// calc vector from stopping point to circle center
Vector2f vec; // vector from circle center to current location
vec.x = (curr_pos.x - _center.x);
vec.y = (curr_pos.y - _center.y);
vec.x = (stopping_point.x - _center.x);
vec.y = (stopping_point.y - _center.y);
float dist = norm(vec.x, vec.y);
// if current location is exactly at the center of the circle return edge directly behind vehicle