From 730cf0ad9bec167e37644f6a559f8e137bd07e77 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Thu, 3 Jun 2021 00:26:39 +0530 Subject: [PATCH] AC_Avoid: Add deadzone param to avoidance backaway --- libraries/AC_Avoidance/AC_Avoid.cpp | 14 +++++++++++--- libraries/AC_Avoidance/AC_Avoid.h | 2 +- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 749499dbd8..86aba19ee4 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/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,14 +1171,14 @@ 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); const float z_back_dist = margin_vector.z; calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, xy_back_dist, vector_to_obstacle, kP_z, accel_cmss_z, z_back_dist, min_back_vel_z, max_back_vel_z, dt); - } + } } if (desired_vel_cms.is_zero()) { diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 89a9cf1dc2..b27c6d7116 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/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