From 368245017a0c8d9fc511a80ebd5aba300aa627c1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 21 Nov 2017 10:48:09 +0900 Subject: [PATCH] AC_WPNav: access pos-controller's horizontal p object --- 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 98dd5ebf61..c4cb620f63 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -302,7 +302,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) // Limit the velocity to prevent fence violations if (_avoid != nullptr) { - _avoid->adjust_velocity(_pos_control.get_pos_xy_kP(), _loiter_accel_cmss, desired_vel, nav_dt); + _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _loiter_accel_cmss, desired_vel, nav_dt); } // send adjusted feed forward velocity back to position controller @@ -631,7 +631,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) // calculate point at which velocity switches from linear to sqrt float linear_velocity = _wp_speed_cms; - float kP = _pos_control.get_pos_xy_kP(); + float kP = _pos_control.get_pos_xy_p().kP(); if (kP >= 0.0f) { // avoid divide by zero linear_velocity = _track_accel/kP; }