From fdd39ca3a85e7896e2c800861f41c32809f526dc Mon Sep 17 00:00:00 2001
From: Rishabh <f20171602@hyderabad.bits-pilani.ac.in>
Date: Tue, 9 Feb 2021 23:26:15 +0530
Subject: [PATCH] Copter: Check for height before turning on proximity simple
 avoidance

---
 ArduCopter/Copter.cpp    |  3 +++
 ArduCopter/Copter.h      |  3 +++
 ArduCopter/avoidance.cpp | 20 ++++++++++++++++++++
 3 files changed, 26 insertions(+)
 create mode 100644 ArduCopter/avoidance.cpp

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 464b8636a2..0093ba7263 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -476,6 +476,9 @@ void Copter::three_hz_loop()
 
     // update ch6 in flight tuning
     tuning();
+
+    // check if avoidance should be enabled based on alt
+    low_alt_avoidance();
 }
 
 // one_hz_loop - runs at 1Hz
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 276752a67b..79656bfc91 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -665,6 +665,9 @@ private:
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
 
+    // avoidance.cpp
+    void low_alt_avoidance();
+
 #if HAL_ADSB_ENABLED
     // avoidance_adsb.cpp
     void avoidance_adsb_update(void);
diff --git a/ArduCopter/avoidance.cpp b/ArduCopter/avoidance.cpp
new file mode 100644
index 0000000000..76400a4b23
--- /dev/null
+++ b/ArduCopter/avoidance.cpp
@@ -0,0 +1,20 @@
+#include "Copter.h"
+
+// check if proximity type Simple Avoidance should be enabled based on alt
+void Copter::low_alt_avoidance()
+{
+#if AC_AVOID_ENABLED == ENABLED
+    int32_t alt_cm;
+    if (!get_rangefinder_height_interpolated_cm(alt_cm)) {
+        // enable avoidance if we don't have a valid rangefinder reading
+        avoid.proximity_alt_avoidance_enable(true);
+        return;
+    }
+
+    bool enable_avoidance = true;
+    if (alt_cm < avoid.get_min_alt() * 100.0f) {
+        enable_avoidance = false;
+    }
+    avoid.proximity_alt_avoidance_enable(enable_avoidance);
+#endif
+}