From 6546ccbb3f4c4dba033e0cfd62ee91f5699c92b7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 20 Nov 2017 20:26:32 +0900 Subject: [PATCH] Copter: velocity pi moved to position control library --- ArduCopter/Copter.h | 7 ++++--- ArduCopter/Parameters.cpp | 29 +++++------------------------ ArduCopter/Parameters.h | 6 +----- ArduCopter/config.h | 16 ---------------- ArduCopter/system.cpp | 2 +- ArduCopter/tuning.cpp | 4 ++-- 6 files changed, 13 insertions(+), 51 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 464232f085..261ea101ce 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -48,10 +48,11 @@ #include #include #include // Mission command library -#include // PID library -#include // PID library (2-axis) -#include // Heli specific Rate PID library #include // P library +#include // PID library +#include // PI library (2-axis) +#include // PID library (2-axis) +#include // Heli specific Rate PID library #include // Attitude control library #include // Attitude control library for traditional helicopter #include // Position control library diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 1520da3aa8..e0f37c5045 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -555,29 +555,6 @@ const AP_Param::Info Copter::var_info[] = { // @User: Advanced GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_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 - 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 @@ -1142,7 +1119,11 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, - { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" } + { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, + { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, + { Parameters::k_param_pi_vel_xy, 1, AP_PARAM_FLOAT, "PSC_VELXY_I" }, + { Parameters::k_param_pi_vel_xy, 2, AP_PARAM_FLOAT, "PSC_VELXY_IMAX" }, + { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_VELXY_FILT" }, }; const AP_Param::ConversionInfo throttle_conversion_info[] = { { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7ac58176ce..b37bac9da5 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -358,7 +358,7 @@ public: k_param_acro_yaw_p, k_param_autotune_axis_bitmask, k_param_autotune_aggressiveness, - k_param_pi_vel_xy, + k_param_pi_vel_xy, // remove k_param_fs_ekf_action, k_param_rtl_climb_min, k_param_rpm_sensor, @@ -469,8 +469,6 @@ public: AP_Float acro_rp_expo; // PI/D controllers - AC_PI_2D pi_vel_xy; - AC_P p_vel_z; AC_PID pid_accel_z; @@ -487,8 +485,6 @@ public: Parameters() : // 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, 0), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 66b65f2c12..559056418e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -477,22 +477,6 @@ # define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode #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 // diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 4fc27e29e1..f0ac66c17d 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -599,7 +599,7 @@ void Copter::allocate_motors(void) pos_control = new AC_PosControl(*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_pos_xy); if (pos_control == nullptr) { AP_HAL::panic("Unable to allocate PosControl"); } diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index fb763d0560..4caecdc470 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -88,11 +88,11 @@ void Copter::tuning() { break; case TUNING_VEL_XY_KP: - g.pi_vel_xy.kP(tuning_value); + pos_control->get_vel_xy_pid().kP(tuning_value); break; case TUNING_VEL_XY_KI: - g.pi_vel_xy.kI(tuning_value); + pos_control->get_vel_xy_pid().kI(tuning_value); break; case TUNING_WP_SPEED: