Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

WPNav: bug fix to reported distance to target

Browse Source 
This value is for reporting purposes only
This commit is contained in:
Randy Mackay 2013-07-10 18:20:05 +09:00
parent 518eba0729
commit 204f613467
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -531,9 +531,10 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
dist_error.y = _target.y - curr.y;
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
_distance_to_target = linear_distance; // for reporting purposes
dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
_distance_to_target = dist_error_total; // for reporting purposes
if( dist_error_total > 2.0f*linear_distance ) {
vel_sqrt = safe_sqrt(2.0f*_wp_accel_cms*(dist_error_total-linear_distance));
desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;