AC_Avoid: Add deadzone param to avoidance backaway

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -97,6 +97,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ACCEL_MAX", 8, AC_Avoid, _accel_max, 3.0f),
 
+    // @Param: BACKUP_DZ
+    // @DisplayName: Avoidance deadzone between stopping and backing away from obstacle
+    // @Description: Distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles. Increase this parameter if you see vehicle going back and forth in front of obstacle.
+    // @Units: m
+    // @Range: 0 2
+    // @User: Standard
+    AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone, 0.10f),
+
     AP_GROUPEND
 };
 
@@ -1163,8 +1171,8 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
         if (is_negative(dist_to_boundary - margin_cm)) {
             const float breach_dist = margin_cm - dist_to_boundary;
             // add a deadzone so that the vehicle doesn't backup and go forward again and again
-            // the deadzone is hardcoded to be 10cm
-            if (breach_dist > AC_AVOID_MIN_BACKUP_BREACH_DIST) {
+            const float deadzone = MAX(0.0f, _backup_deadzone) * 100.0f;
+            if (breach_dist > deadzone) {
                 // this vector will help us decide how much we have to back away horizontally and vertically
                 const Vector3f margin_vector = vector_to_obstacle.normalized() * breach_dist;
                 const float xy_back_dist = norm(margin_vector.x, margin_vector.y);

libraries/AC_Avoidance/AC_Avoid.h

@@ -19,7 +19,6 @@
 #define AC_AVOID_ANGLE_MAX_PERCENT          0.75f   // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle
 
 #define AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS    500     // if limiting is active if last limit is happend in the last x ms
-#define AC_AVOID_MIN_BACKUP_BREACH_DIST     10.0f   // vehicle will backaway if breach is greater than this distance in cm
 #define AC_AVOID_ACCEL_TIMEOUT_MS           200     // stored velocity used to calculate acceleration will be reset if avoidance is active after this many ms
 
 /*
@@ -216,6 +215,7 @@ private:
     AP_Float _backup_speed_max; // Maximum speed that will be used to back away (in m/s)
     AP_Float _alt_min;          // alt below which Proximity based avoidance is turned off
     AP_Float _accel_max;        // maximum accelration while simple avoidance is active
+    AP_Float _backup_deadzone;  // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles
 
     bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
     bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude