AC_Avoidance: add 2m margin to upward avoidance

libraries/AC_Avoidance/AC_Avoid.cpp

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

libraries/AC_Avoidance/AC_Avoid.h

@@ -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).