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

AC_Circle: get-closest-point-on-circle returns dist

This commit is contained in:
Randy Mackay 2024-09-11 20:23:30 +09:00
parent 9ca47cf465
commit 2753b7030b
2 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -234,27 +234,28 @@ bool AC_Circle::update(float climb_rate_cms)
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// closest point on the circle will be placed in result, dist_cm will be updated with the 3D distance to the center
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) const
{
// get current position
Vector3p stopping_point;
_pos_control.get_stopping_point_xy_cm(stopping_point.xy());
_pos_control.get_stopping_point_z_cm(stopping_point.z);
// calc vector from stopping point to circle center
Vector3f vec = (stopping_point - _center).tofloat();
dist_cm = vec.length();
// return center if radius is zero
if (!is_positive(_radius)) {
result = _center.tofloat();
return;
}
// get current position
Vector2p stopping_point;
_pos_control.get_stopping_point_xy_cm(stopping_point);
// calc vector from stopping point to circle center
Vector2f vec = (stopping_point - _center.xy()).tofloat();
float dist = vec.length();
// if current location is exactly at the center of the circle return edge directly behind vehicle
if (is_zero(dist)) {
if (is_zero(dist_cm)) {
result.x = _center.x - _radius * _ahrs.cos_yaw();
result.y = _center.y - _radius * _ahrs.sin_yaw();
result.z = _center.z;
@ -262,8 +263,8 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
}
// calculate closest point on edge of circle
result.x = _center.x + vec.x / dist * _radius;
result.y = _center.y + vec.y / dist * _radius;
result.x = _center.x + vec.x / dist_cm * _radius;
result.y = _center.y + vec.y / dist_cm * _radius;
result.z = _center.z;
}

4
libraries/AC_WPNav/AC_Circle.h
View File

@ -75,10 +75,10 @@ public:
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// closest point on the circle will be placed in result, dist_cm will be updated with the distance to the center
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void get_closest_point_on_circle(Vector3f &result) const;
void get_closest_point_on_circle(Vector3f& result, float& dist_cm) const;
/// get horizontal distance to loiter target in cm
float get_distance_to_target() const { return _pos_control.get_pos_error_xy_cm(); }