Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Add deadzone to backaway code

This commit is contained in:
Rishabh 2020-12-27 22:00:07 +05:30 committed by Randy Mackay
parent 343ba1a693
commit 91132d00d9
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -1078,13 +1078,17 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
// back away if vehicle has breached margin
if (is_negative(dist_to_boundary - margin_cm)) {
const float breach_dist = margin_cm - dist_to_boundary;
// this vector will help us divide 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);
// 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) {
// this vector will help us divide 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()) {
// cannot limit velocity if there is nothing to limit
// backing up (if needed) has already been done

1
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -19,6 +19,7 @@
#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
/*
* This class prevents the vehicle from leaving a polygon fence in