From 395a4ad4ce69ebadcc7908129391190d77a7a012 Mon Sep 17 00:00:00 2001 From: Sharvashish Das Date: Wed, 22 Nov 2017 20:19:55 +0530 Subject: [PATCH] Copter: add z-axis obj avoidance to guided vel control added code to adjust z velocity using avoidance --- ArduCopter/control_guided.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index b389b06cce..81dcc5b332 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -672,11 +672,6 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto // get current desired velocity Vector3f curr_vel_des = pos_control->get_desired_velocity(); - // exit immediately if already equal - if (curr_vel_des == vel_des) { - return; - } - // get change in desired velocity Vector3f vel_delta = vel_des - curr_vel_des; @@ -697,6 +692,8 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto #if AC_AVOID_ENABLED // limit the velocity to prevent fence violations avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des); + // get avoidance adjusted climb rate + curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); #endif // update position controller with new target