From cef50d8a30038f0d1cce703e751620c195eddbcf Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 20 Nov 2017 21:36:46 +0900
Subject: [PATCH] Sub: velocity pi moved to position control library

---
 ArduSub/Parameters.cpp | 31 -------------------------------
 ArduSub/Parameters.h   |  6 +-----
 ArduSub/Sub.cpp        |  3 +--
 ArduSub/Sub.h          |  5 +++--
 ArduSub/config.h       | 16 ----------------
 5 files changed, 5 insertions(+), 56 deletions(-)

diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index ddc828f5e0..80b76a9df1 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -451,37 +451,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Advanced
     GSCALAR(acro_expo,  "ACRO_EXPO",    ACRO_EXPO_DEFAULT),
 
-    // @Param: VEL_XY_P
-    // @DisplayName: Velocity (horizontal) P gain
-    // @Description: Velocity (horizontal) P gain.  Converts the difference between desired velocity to a target acceleration
-    // @Range: 0.1 6.0
-    // @Increment: 0.1
-    // @User: Advanced
-
-    // @Param: VEL_XY_I
-    // @DisplayName: Velocity (horizontal) I gain
-    // @Description: Velocity (horizontal) I gain.  Corrects long-term difference in desired velocity to a target acceleration
-    // @Range: 0.02 1.00
-    // @Increment: 0.01
-    // @User: Advanced
-
-    // @Param: VEL_XY_IMAX
-    // @DisplayName: Velocity (horizontal) integrator maximum
-    // @Description: Velocity (horizontal) integrator maximum.  Constrains the target acceleration that the I gain will output
-    // @Range: 0 4500
-    // @Increment: 10
-    // @Units: cm/s/s
-    // @User: Advanced
-
-    // @Param: VEL_XY_FILT_HZ
-    // @DisplayName: Velocity (horizontal) integrator maximum
-    // @Description: Velocity (horizontal) integrator maximum.  Constrains the target acceleration that the I gain will output
-    // @Range: 0 4500
-    // @Increment: 10
-    // @Units: cm/s/s
-    // @User: Advanced
-    GGROUP(pi_vel_xy,   "VEL_XY_",  AC_PI_2D),
-
     // @Param: VEL_Z_P
     // @DisplayName: Velocity (vertical) P gain
     // @Description: Velocity (vertical) P gain.  Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index 3031ff6e6c..a51a680b96 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -151,7 +151,7 @@ public:
         // PID Controllers
         k_param_p_pos_xy = 126,
         k_param_p_alt_hold,
-        k_param_pi_vel_xy,
+        k_param_pi_vel_xy, // deprecated
         k_param_p_vel_z,
         k_param_pid_accel_z,
 
@@ -305,8 +305,6 @@ public:
     AP_Float        acro_expo;
 
     // PI/D controllers
-    AC_PI_2D        pi_vel_xy;
-
     AC_P            p_vel_z;
     AC_PID          pid_accel_z;
 
@@ -322,8 +320,6 @@ public:
 
         // PID controller       initial P         initial I         initial D       initial imax        initial filt hz     pid rate
         //---------------------------------------------------------------------------------------------------------------------------------
-        pi_vel_xy(VEL_XY_P,        VEL_XY_I,                         VEL_XY_IMAX,        VEL_XY_FILT_HZ,     WPNAV_LOITER_UPDATE_TIME),
-
         p_vel_z(VEL_Z_P),
         pid_accel_z(ACCEL_Z_P,       ACCEL_Z_I,        ACCEL_Z_D,      ACCEL_Z_IMAX,       ACCEL_Z_FILT_HZ,    MAIN_LOOP_SECONDS),
 
diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index e8d3215395..0c55187995 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -49,8 +49,7 @@ Sub::Sub(void)
           ahrs_view(ahrs, ROTATION_NONE),
           attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS),
           pos_control(ahrs_view, 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),
+                      g.p_alt_hold, g.p_vel_z, g.pid_accel_z, g.p_pos_xy),
           wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           circle_nav(inertial_nav, ahrs_view, pos_control),
           pmTest1(0),
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 9010aaf523..6473d8c055 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -48,9 +48,10 @@
 #include <AP_NavEKF2/AP_NavEKF2.h>
 #include <AP_NavEKF3/AP_NavEKF3.h>
 #include <AP_Mission/AP_Mission.h>         // Mission command library
-#include <AC_PID/AC_PID.h>             // PID library
-#include <AC_PID/AC_PI_2D.h>           // PID library (2-axis)
 #include <AC_PID/AC_P.h>               // P library
+#include <AC_PID/AC_PID.h>             // PID library
+#include <AC_PID/AC_PI_2D.h>           // PI library (2-axis)
+#include <AC_PID/AC_PID_2D.h>          // PID library (2-axis)
 #include <AC_AttitudeControl/AC_AttitudeControl_Sub.h> // Attitude control library
 #include <AC_AttitudeControl/AC_PosControl_Sub.h>      // Position control library
 #include <RC_Channel/RC_Channel.h>         // RC Channel Library
diff --git a/ArduSub/config.h b/ArduSub/config.h
index bae4edba8e..733f79e1cf 100644
--- a/ArduSub/config.h
+++ b/ArduSub/config.h
@@ -231,22 +231,6 @@
 # define POS_XY_P               1.0f
 #endif
 
-//////////////////////////////////////////////////////////////////////////////
-// Velocity (horizontal) gains
-//
-#ifndef VEL_XY_P
-# define VEL_XY_P              1.0f
-#endif
-#ifndef VEL_XY_I
-# define VEL_XY_I              0.5f
-#endif
-#ifndef VEL_XY_IMAX
-# define VEL_XY_IMAX           1000
-#endif
-#ifndef VEL_XY_FILT_HZ
-# define VEL_XY_FILT_HZ        5.0f
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // PosHold parameter defaults
 //