AP_Avoidance: Change the determination place of the index value.

libraries/AP_Avoidance/AP_Avoidance.cpp

@@ -213,15 +213,15 @@ void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
         } else if (oldest_timestamp < obstacle_timestamp_ms) {
             // replace this very old entry with this new data
             index = oldest_index;
+        } else {
+            // no room for this (old?!) data
+            return;
         }
+
         _obstacles[index].src = src;
         _obstacles[index].src_id = src_id;
     }
 
-    if (index == -1) {
-        // no room for this (old?!) data
-        return;
-    }
     _obstacles[index]._location = loc;
     _obstacles[index]._velocity = vel_ned;
     _obstacles[index].timestamp_ms = obstacle_timestamp_ms;