From 45de3b11639c722df4d72ef874467d47725db24f Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Sun, 4 Dec 2016 11:49:46 -0500
Subject: [PATCH] Sub: Disable Avoidance and Proximity by default

---
 ArduSub/APM_Config.h   |  2 ++
 ArduSub/Parameters.cpp |  6 +++++-
 ArduSub/Parameters.h   |  2 +-
 ArduSub/Sub.cpp        |  2 ++
 ArduSub/Sub.h          | 16 ++++++++++++++--
 ArduSub/system.cpp     |  3 +++
 6 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h
index c136680b9f..61acb3bda0 100644
--- a/ArduSub/APM_Config.h
+++ b/ArduSub/APM_Config.h
@@ -28,6 +28,8 @@
 //#define NAV_GUIDED            DISABLED            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
 
 // features below are disabled by default on all boards
+//#define AVOIDANCE_ENABLED		ENABLED
+//#define PROXIMITY_ENABLED		ENABLED
 //#define AC_RALLY              ENABLED            // enable rally points library
 //#define AC_TERRAIN            ENABLED            // enable terrain library (Must also enable Rally)
 //#define OPTFLOW               ENABLED // enable optical flow sensor support
diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index 96898d00a9..515f86d1c1 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -822,9 +822,11 @@ const AP_Param::Info Sub::var_info[] = {
     GOBJECT(fence,      "FENCE_",   AC_Fence),
 #endif
 
+#if AVOIDANCE_ENABLED == ENABLED
 	// @Group: AVOID_
     // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
     GOBJECT(avoid,      "AVOID_",   AC_Avoid),
+#endif
 
 #if AC_RALLY == ENABLED
     // @Group: RALLY_
@@ -958,7 +960,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
   constructor for g2 object
  */
 ParametersG2::ParametersG2(void)
-    : proximity(sub.serial_manager)
+#if PROXIMITY_ENABLED == ENABLED
+   : proximity(sub.serial_manager)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index 4acd6cb731..309097682a 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -96,7 +96,7 @@ public:
         k_param_terrain, // Terrain database
         k_param_rally, // Disabled
         k_param_circle_nav, // Disabled
-		k_param_avoid,
+		k_param_avoid, // Relies on proximity and fence
 
 
 		// Other external hardware interfaces
diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index f363f27336..6f01c9cf0c 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -65,7 +65,9 @@ Sub::Sub(void) :
     pos_control(ahrs, inertial_nav, motors, attitude_control,
                 g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
                 g.p_pos_xy, g.pi_vel_xy),
+#if AVOIDANCE_ENABLED == ENABLED
 	avoid(ahrs, inertial_nav, fence, g2.proximity),
+#endif
     wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
     circle_nav(inertial_nav, ahrs, pos_control),
     pmTest1(0),
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 365cae5090..3334ce54e7 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -64,7 +64,7 @@
 #include <RC_Channel/RC_Channel.h>         // RC Channel Library
 #include <AP_Motors/AP_Motors.h>          // AP Motors library
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
-#include <AP_Proximity/AP_Proximity.h>
+
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <Filter/Filter.h>             // Filter library
 #include <AP_Buffer/AP_Buffer.h>          // APM FIFO Buffer
@@ -79,7 +79,7 @@
 #include <AC_WPNav/AC_Circle.h>          // circle navigation library
 #include <AP_Declination/AP_Declination.h>     // ArduPilot Mega Declination Helper Library
 #include <AC_Fence/AC_Fence.h>           // ArduCopter Fence library
-#include <AC_Avoidance/AC_Avoid.h>           // Arducopter stop at fence library
+
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
 #include <AP_Notify/AP_Notify.h>          // Notify library
 #include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
@@ -106,6 +106,14 @@
 #include <AP_Gripper/AP_Gripper.h>             // gripper stuff
 #endif
 
+#if PROXIMITY_ENABLED == ENABLED
+#include <AP_Proximity/AP_Proximity.h>
+#endif
+
+#if AVOIDANCE_ENABLED == ENABLED
+#include <AC_Avoidance/AC_Avoid.h>           // Arducopter stop at fence library
+#endif
+
 // Local modules
 #include "Parameters.h"
 
@@ -417,7 +425,11 @@ private:
     AC_AttitudeControl_Multi attitude_control;
 
     AC_PosControl pos_control;
+
+#if AVOIDANCE_ENABLED == ENABLED
     AC_Avoid avoid;
+#endif
+
     AC_WPNav wp_nav;
     AC_Circle circle_nav;
 
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index ced09eb0bf..19c2ee6d60 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -183,7 +183,10 @@ void Sub::init_ardupilot()
 	Location_Class::set_terrain(&terrain);
 	wp_nav.set_terrain(&terrain);
 #endif
+
+#if AVOIDANCE_ENABLED == ENABLED
 	wp_nav.set_avoidance(&avoid);
+#endif
 
     pos_control.set_dt(MAIN_LOOP_SECONDS);