From 5092bc2f6d38b494f7bf6e284581b1f6135baae2 Mon Sep 17 00:00:00 2001
From: Rishabh <f20171602@hyderabad.bits-pilani.ac.in>
Date: Sun, 27 Dec 2020 22:02:34 +0530
Subject: [PATCH] AC_Avoid: Simplify accessing boundary

---
 libraries/AC_Avoidance/AC_Avoid.cpp | 5 ++++-
 libraries/AC_Avoidance/AC_Avoid.h   | 3 +--
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp
index 23064bf728..c4deed19d8 100644
--- a/libraries/AC_Avoidance/AC_Avoid.cpp
+++ b/libraries/AC_Avoidance/AC_Avoid.cpp
@@ -1072,7 +1072,10 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
     for (uint8_t i = 0; i<obstacle_num; i++) {
         // get obstacle from proximity library
         Vector3f vector_to_obstacle;
-        _proximity.get_obstacle(i, vector_to_obstacle);
+        if (!_proximity.get_obstacle(i, vector_to_obstacle)) {
+            // this one is not valid
+            continue;
+        }
         const float dist_to_boundary = vector_to_obstacle.length();
         if (is_zero(dist_to_boundary)) {
             continue;
diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h
index ea038764b6..0af814bc75 100644
--- a/libraries/AC_Avoidance/AC_Avoid.h
+++ b/libraries/AC_Avoidance/AC_Avoid.h
@@ -22,8 +22,7 @@
 #define AC_AVOID_MIN_BACKUP_BREACH_DIST     10.0f   // vehicle will backaway if breach is greater than this distance in cm
 
 /*
- * This class prevents the vehicle from leaving a polygon fence in
- * 2 dimensions by limiting velocity (adjust_velocity).
+ * This class prevents the vehicle from leaving a polygon fence or avoiding proximity based obstacles
  * Additionally the vehicle may back up if the margin to obstacle is breached
  */
 class AC_Avoid {