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

@@ -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;
         }
     }
 }