Copter: Check for height before turning on proximity simple avoidance

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

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);

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
+}