From 1b1e1fd32ebb884f3225f9eb70adbe6951b5111c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 10 Jul 2013 18:20:05 +0900
Subject: [PATCH] WPNav: bug fix to reported distance to target

This value is for reporting purposes only
---
 libraries/AC_WPNav/AC_WPNav.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 51daa3ce8..8d8ecde4f 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -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;
-- 
GitLab