From c4f17b32357a18ca8359e2811d73e9e71b543b96 Mon Sep 17 00:00:00 2001 From: Randy Mackay 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 5ae5c0df89..02743150aa 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