From b5bdaa18ac1edbc884ef1c43cd168eb4c90298e1 Mon Sep 17 00:00:00 2001
From: Matt <lawrence.matt@gmail.com>
Date: Wed, 27 Dec 2017 21:44:34 -0500
Subject: [PATCH] AP_AVOIDANCE: Minimum altitude for avoidance action

---
 libraries/AP_Avoidance/AP_Avoidance.cpp | 15 ++++++++++++++-
 libraries/AP_Avoidance/AP_Avoidance.h   |  1 +
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp
index 53948cdfdc..921cd2303d 100644
--- a/libraries/AP_Avoidance/AP_Avoidance.cpp
+++ b/libraries/AP_Avoidance/AP_Avoidance.cpp
@@ -114,6 +114,13 @@ const AP_Param::GroupInfo AP_Avoidance::var_info[] = {
     // @Units: m
     // @User: Advanced
     AP_GROUPINFO("F_DIST_Z",    11, AP_Avoidance, _fail_distance_z, AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT),
+    
+    // @Param: F_ALT_MIN
+    // @DisplayName: ADS-B avoidance minimum altitude
+    // @Description: Minimum altitude for ADS-B avoidance. If the vehicle is below this altitude, no avoidance action will take place. Useful to prevent ADS-B avoidance from activating while below the tree line or around structures. Default of 0 is no minimum.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("F_ALT_MIN",    12, AP_Avoidance, _fail_altitude_minimum, 0),
 
     AP_GROUPEND
 };
@@ -511,7 +518,13 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
         new_threat_level = threat->threat_level;
         if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
             action = (MAV_COLLISION_ACTION)_fail_action.get();
-        }
+            Location my_loc;
+            if (action != MAV_COLLISION_ACTION_NONE && _fail_altitude_minimum > 0 &&
+             _ahrs.get_position(my_loc) && ((my_loc.alt*0.01f) < _fail_altitude_minimum)) {
+                // disable avoidance when close to ground, report only
+                action = MAV_COLLISION_ACTION_REPORT;
+			}
+		}
     }
 
     uint32_t now = AP_HAL::millis();
diff --git a/libraries/AP_Avoidance/AP_Avoidance.h b/libraries/AP_Avoidance/AP_Avoidance.h
index ee38b7cdb2..bb7d3581c5 100644
--- a/libraries/AP_Avoidance/AP_Avoidance.h
+++ b/libraries/AP_Avoidance/AP_Avoidance.h
@@ -189,6 +189,7 @@ private:
     AP_Int8     _fail_time_horizon;
     AP_Int16    _fail_distance_xy;
     AP_Int16    _fail_distance_z;
+    AP_Int16    _fail_altitude_minimum;
 
     AP_Int8     _warn_action;
     AP_Int8     _warn_time_horizon;