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

AC_Avoidance: add 2m margin to upward avoidance

This commit is contained in:
Randy Mackay 2017-02-10 16:12:22 +09:00
parent d170396acf
commit 49f4afc2a1
2 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -107,7 +107,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
// get distance from proximity sensor (in meters, convert to cm)
float proximity_alt_diff_m;
if (_proximity.get_upward_distance(proximity_alt_diff_m)) {
float proximity_alt_diff_cm = proximity_alt_diff_m * 100.0f;
float proximity_alt_diff_cm = (proximity_alt_diff_m - AC_AVOID_UPWARD_MARGIN_M) * 100.0f;
if (!limit_alt || proximity_alt_diff_cm < alt_diff_cm) {
alt_diff_cm = proximity_alt_diff_cm;
}

2
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -21,6 +21,8 @@
#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 10.0f // objects over 10m away are ignored (default value for DIST_MAX parameter)
#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_UPWARD_MARGIN_M 2.0f // stop 2m before objects above the vehicle
/*
* This class prevents the vehicle from leaving a polygon fence in
* 2 dimensions by limiting velocity (adjust_velocity).