From 29a75be4b1516e4cc2700bacf5aa9e2cbc42487b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 10 Jul 2013 16:23:51 +0900
Subject: [PATCH] WPNav: rounding error fix in loiter

Contributed by Leonard Hall
---
 libraries/AC_WPNav/AC_WPNav.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 5ae5c0df8..02743150a 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -535,8 +535,8 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
             desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
             desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
         }else{
-            desired_vel.x = _pid_pos_lat->get_p(dist_error.x);
-            desired_vel.y = _pid_pos_lon->get_p(dist_error.y);
+            desired_vel.x = _pid_pos_lat->kP() * dist_error.x;
+            desired_vel.y = _pid_pos_lon->kP() * dist_error.y;
         }
 
         // ensure velocity stays within limits
-- 
GitLab