From d359c5ddf20ea2ca8379844224d5e69b9c493b3a Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sun, 27 Dec 2020 22:01:05 +0530 Subject: [PATCH] AC_Avoid: Let user take control if they are backing away at a greater speed than what we have calculated --- libraries/AC_Avoidance/AC_Avoid.cpp | 33 ++++++++++++++++------------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 68cc9fd1aa..23064bf728 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -193,26 +193,29 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel if (desired_backup_vel.length() > max_back_spd_cms) { desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms; } - - if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { - // project desired velocity towards backup velocity horizontally - const Vector2f backup_vel_xy{desired_backup_vel.x, desired_backup_vel.y}; - Vector2f projected_vel; - if (!backup_vel_xy.is_zero()) { - projected_vel = Vector2f{desired_vel_cms.x, desired_vel_cms.y}.projected(Vector2f{desired_backup_vel.x, desired_backup_vel.y}); + + // let user take control if they are backing away at a greater speed than what we have calculated + // this has to be done for x,y,z seperately. For eg, user is doing fine in "x" direction but might need backing up in "y". + if (!is_zero(desired_backup_vel.x)) { + if (is_positive(desired_backup_vel.x)) { + desired_vel_cms.x = MAX(desired_vel_cms.x, desired_backup_vel.x); + } else { + desired_vel_cms.x = MIN(desired_vel_cms.x, desired_backup_vel.x); } - // subtract this projection since we are already going in that direction - desired_vel_cms -= Vector3f{projected_vel.x, projected_vel.y, 0.0f}; - desired_vel_cms += Vector3f{desired_backup_vel.x, desired_backup_vel.y, 0.0f}; - // let user take control vertically if they are backing away at a greater speed than what we have calculated based on limits - if (!is_negative(desired_backup_vel.z)) { + } + if (!is_zero(desired_backup_vel.y)) { + if (is_positive(desired_backup_vel.y)) { + desired_vel_cms.y = MAX(desired_vel_cms.y, desired_backup_vel.y); + } else { + desired_vel_cms.y = MIN(desired_vel_cms.y, desired_backup_vel.y); + } + } + if (!is_zero(desired_backup_vel.z)) { + if (is_positive(desired_backup_vel.z)) { desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z); } else { desired_vel_cms.z = MIN(desired_vel_cms.z, desired_backup_vel.z); } - } else { - // back away to stopping position - desired_vel_cms = desired_backup_vel; } } }