From 401d3654915c8cd3beb37b08d42c1b3317b36a6c Mon Sep 17 00:00:00 2001 From: Rishabh Date: Wed, 27 Jan 2021 21:52:24 +0530 Subject: [PATCH] AC_WPNav: Adapt to 3-D Avoidance changes --- libraries/AC_WPNav/AC_Loiter.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 2477f11820..64e2eb56fb 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -307,7 +307,9 @@ void AC_Loiter::calc_desired_velocity(float nav_dt) // TODO: We need to also limit the _desired_accel AC_Avoid *_avoid = AP::ac_avoid(); if (_avoid != nullptr) { - _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt); + Vector3f avoidance_vel_3d{desired_vel.x, desired_vel.y, 0.0f}; + _avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_xy_p().kP(), _accel_cmss, _pos_control.get_pos_z_p().kP(), _pos_control.get_max_accel_z(), nav_dt); + desired_vel = Vector2f{avoidance_vel_3d.x, avoidance_vel_3d.y}; } // send adjusted feed forward acceleration and velocity back to the Position Controller