Copter: integrate AC_Avoidance library

ArduCopter/Copter.cpp

@@ -76,6 +76,7 @@ Copter::Copter(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),
+    avoid(ahrs, inertial_nav, fence),
     wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
     circle_nav(inertial_nav, ahrs, pos_control),
     pmTest1(0),

ArduCopter/Copter.h

@@ -76,6 +76,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_RCMapper/AP_RCMapper.h>        // RC input mapping library
 #include <AP_Notify/AP_Notify.h>          // Notify library
@@ -458,6 +459,7 @@ private:
     AC_AttitudeControl_Multi attitude_control;
 #endif
     AC_PosControl pos_control;
+    AC_Avoid avoid;
     AC_WPNav wp_nav;
     AC_Circle circle_nav;
 

ArduCopter/Parameters.cpp

@@ -825,6 +825,10 @@ const AP_Param::Info Copter::var_info[] = {
     GOBJECT(fence,      "FENCE_",   AC_Fence),
 #endif
 
+    // @Group: AVOID_
+    // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
+    GOBJECT(avoid,      "AVOID_",   AC_Avoid),
+
 #if AC_RALLY == ENABLED
     // @Group: RALLY_
     // @Path: ../libraries/AP_Rally/AP_Rally.cpp

ArduCopter/Parameters.h

@@ -181,6 +181,7 @@ public:
         k_param_fs_crash_check,
         k_param_throw_motor_start,
         k_param_terrain_follow,    // 94
+        k_param_avoid,
 
         // 97: RSSI
         k_param_rssi = 97,

ArduCopter/system.cpp

@@ -200,6 +200,7 @@ void Copter::init_ardupilot()
     Location_Class::set_terrain(&terrain);
     wp_nav.set_terrain(&terrain);
 #endif
+    wp_nav.set_avoidance(&avoid);
 
     pos_control.set_dt(MAIN_LOOP_SECONDS);