From bd37eab3afab6f58dd2a88c3fa191095f6811f77 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sun, 6 Dec 2020 18:18:21 +0530 Subject: [PATCH] Copter: Support 3D Simple Avoidance --- ArduCopter/mode_follow.cpp | 10 +++------- ArduCopter/mode_guided.cpp | 6 ++---- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index a17212c465..e8d225993b 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -93,11 +93,7 @@ void ModeFollow::run() // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down) const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length(); - copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt); - - // limit the horizontal velocity to prevent fence violations - copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt); - + copter.avoid.limit_velocity_2D(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt); // copy horizontal velocity limits back to 3d vector desired_velocity_neu_cms.x = desired_velocity_xy_cms.x; desired_velocity_neu_cms.y = desired_velocity_xy_cms.y; @@ -106,8 +102,8 @@ void ModeFollow::run() const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt); desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max); - // get avoidance adjusted climb rate - desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z); + // limit the velocity for obstacle/fence avoidance + copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z(), G_Dt); // calculate vehicle heading switch (g2.follow.get_yaw_behave()) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index e487032728..a67b853c0a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -679,10 +679,8 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max); #if AC_AVOID_ENABLED - // limit the velocity to prevent fence violations - copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt); - // get avoidance adjusted climb rate - curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); + // limit the velocity for obstacle/fence avoidance + copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); #endif // update position controller with new target