From cef50d8a30038f0d1cce703e751620c195eddbcf Mon Sep 17 00:00:00 2001 From: Randy Mackay 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 #include #include // Mission command library -#include // PID library -#include // PID library (2-axis) #include // P library +#include // PID library +#include // PI library (2-axis) +#include // PID library (2-axis) #include // Attitude control library #include // Position control library #include // 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 //