From 489c42bdff21a0278e4eb9a8b298cebddfe20819 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 12 Dec 2017 15:12:16 +0900
Subject: [PATCH] Rover: add avoidance library

---
 APMrover2/AP_Arming.cpp  | 2 +-
 APMrover2/Parameters.cpp | 7 ++++++-
 APMrover2/Parameters.h   | 3 +++
 APMrover2/Rover.h        | 1 +
 APMrover2/make.inc       | 1 +
 APMrover2/wscript        | 1 +
 6 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp
index 960b6c2912..3fb4464da5 100644
--- a/APMrover2/AP_Arming.cpp
+++ b/APMrover2/AP_Arming.cpp
@@ -109,7 +109,7 @@ bool AP_Arming_Rover::proximity_check(bool report)
 
     // get closest object if we might use it for avoidance
     float angle_deg, distance;
-    if (rover.g2.proximity.get_closest_object(angle_deg, distance)) {
+    if (rover.g2.avoid.proximity_avoidance_enabled() && rover.g2.proximity.get_closest_object(angle_deg, distance)) {
         // display error if something is within 60cm
         if (distance <= 0.6f) {
             if (report) {
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index b633ce5d2a..a60c5e24c0 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -550,6 +550,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
     AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity),
 
+    // @Group: AVOID_
+    // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
+    AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid),
+
     AP_GROUPEND
 };
 
@@ -564,7 +568,8 @@ ParametersG2::ParametersG2(void)
     attitude_control(rover.ahrs),
     smart_rtl(rover.ahrs),
     fence(rover.ahrs),
-    proximity(rover.serial_manager)
+    proximity(rover.serial_manager),
+    avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index e6ea0df3f1..f1887921e0 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -336,6 +336,9 @@ public:
 
     // proximity library
     AP_Proximity proximity;
+
+    // avoidance library
+    AC_Avoid avoid;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index ae89fa627a..0a7528a4e9 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -78,6 +78,7 @@
 #include <StorageManager/StorageManager.h>
 #include <AC_Fence/AC_Fence.h>
 #include <AP_Proximity/AP_Proximity.h>
+#include <AC_Avoidance/AC_Avoid.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
 #endif
diff --git a/APMrover2/make.inc b/APMrover2/make.inc
index df392faf34..39bdbab8d4 100644
--- a/APMrover2/make.inc
+++ b/APMrover2/make.inc
@@ -50,3 +50,4 @@ LIBRARIES += AC_PID
 LIBRARIES += AP_Proximity
 LIBRARIES += AC_Fence
 LIBRARIES += AP_SmartRTL
+LIBRARIES += AC_Avoidance
diff --git a/APMrover2/wscript b/APMrover2/wscript
index 85f8a0ef04..0d4aeb1510 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -24,6 +24,7 @@ def build(bld):
             'AP_SmartRTL',
             'AC_Fence',
             'AP_Proximity',
+            'AC_Avoidance',
         ],
     )