diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index f7b694513b..4cbad7dbb7 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -6,6 +6,15 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // default gains for Plane + # define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default + # define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default + # define POSCONTROL_ACCEL_Z_P 0.3f // vertical acceleration controller P gain default + # define POSCONTROL_ACCEL_Z_I 1.0f // vertical acceleration controller I gain default + # define POSCONTROL_ACCEL_Z_D 0.0f // vertical acceleration controller D gain default + # define POSCONTROL_ACCEL_Z_IMAX 800 // vertical acceleration controller IMAX gain default + # define POSCONTROL_ACCEL_Z_FILT_HZ 10.0f // vertical acceleration controller input filter default + # define POSCONTROL_ACCEL_Z_DT 0.02f // vertical acceleration controller dt default + # define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default # define POSCONTROL_VEL_XY_P 0.7f // horizontal velocity controller P gain default # define POSCONTROL_VEL_XY_I 0.35f // horizontal velocity controller I gain default # define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D gain default @@ -14,6 +23,15 @@ extern const AP_HAL::HAL& hal; # define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D #else // default gains for Copter and Sub + # define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default + # define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default + # define POSCONTROL_ACCEL_Z_P 0.5f // vertical acceleration controller P gain default + # define POSCONTROL_ACCEL_Z_I 1.0f // vertical acceleration controller I gain default + # define POSCONTROL_ACCEL_Z_D 0.0f // vertical acceleration controller D gain default + # define POSCONTROL_ACCEL_Z_IMAX 800 // vertical acceleration controller IMAX gain default + # define POSCONTROL_ACCEL_Z_FILT_HZ 20.0f // vertical acceleration controller input filter default + # define POSCONTROL_ACCEL_Z_DT 0.0025f // vertical acceleration controller dt default + # define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default # define POSCONTROL_VEL_XY_P 1.0f // horizontal velocity controller P gain default # define POSCONTROL_VEL_XY_I 0.5f // horizontal velocity controller I gain default # define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D gain default @@ -34,6 +52,61 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @User: Advanced AP_GROUPINFO("_ACC_XY_FILT", 1, AC_PosControl, _accel_xy_filt_hz, POSCONTROL_ACCEL_FILTER_HZ), + // @Param: _POSZ_P + // @DisplayName: Position (vertical) controller P gain + // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller + // @Range: 1.000 3.000 + // @User: Standard + AP_SUBGROUPINFO(_p_pos_z, "_POSZ_", 2, AC_PosControl, AC_P), + + // @Param: _VELZ_P + // @DisplayName: Velocity (vertical) controller P gain + // @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller + // @Range: 1.000 8.000 + // @User: Standard + AP_SUBGROUPINFO(_p_vel_z, "_VELZ_", 3, AC_PosControl, AC_P), + + // @Param: _ACCELZ_P + // @DisplayName: Acceleration (vertical) controller P gain + // @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output + // @Range: 0.500 1.500 + // @Increment: 0.05 + // @User: Standard + + // @Param: _ACCELZ_I + // @DisplayName: Acceleration (vertical) controller I gain + // @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration + // @Range: 0.000 3.000 + // @User: Standard + + // @Param: _ACCELZ_IMAX + // @DisplayName: Acceleration (vertical) controller I gain maximum + // @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate + // @Range: 0 1000 + // @Units: d% + // @User: Standard + + // @Param: _ACCELZ_D + // @DisplayName: Acceleration (vertical) controller D gain + // @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration + // @Range: 0.000 0.400 + // @User: Standard + + // @Param: _ACCELZ_FILT + // @DisplayName: Acceleration (vertical) controller filter + // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. + // @Range: 1.000 100.000 + // @Units: Hz + // @User: Standard + AP_SUBGROUPINFO(_pid_accel_z, "_ACCELZ_", 4, AC_PosControl, AC_PID), + + // @Param: _POSXY_P + // @DisplayName: Position (horizonal) controller P gain + // @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @Range: 0.500 2.000 + // @User: Standard + AP_SUBGROUPINFO(_p_pos_xy, "_POSXY_", 5, AC_PosControl, AC_P), + // @Param: _VELXY_P // @DisplayName: Velocity (horizontal) P gain // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration @@ -76,8 +149,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Range: 0 100 // @Units: Hz // @User: Advanced - - AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 2, AC_PosControl, AC_PID_2D), + AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 6, AC_PosControl, AC_PID_2D), AP_GROUPEND }; @@ -87,17 +159,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // their values. // AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const AP_Motors& motors, AC_AttitudeControl& attitude_control, - AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, - AC_P& p_pos_xy) : + const AP_Motors& motors, AC_AttitudeControl& attitude_control) : _ahrs(ahrs), _inav(inav), _motors(motors), _attitude_control(attitude_control), - _p_pos_z(p_pos_z), - _p_vel_z(p_vel_z), - _pid_accel_z(pid_accel_z), - _p_pos_xy(p_pos_xy), + _p_pos_z(POSCONTROL_POS_Z_P), + _p_vel_z(POSCONTROL_VEL_Z_P), + _pid_accel_z(POSCONTROL_ACCEL_Z_P, POSCONTROL_ACCEL_Z_I, POSCONTROL_ACCEL_Z_D, POSCONTROL_ACCEL_Z_IMAX, POSCONTROL_ACCEL_Z_FILT_HZ, POSCONTROL_ACCEL_Z_DT), + _p_pos_xy(POSCONTROL_POS_XY_P), _pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_50HZ), _dt(POSCONTROL_DT_400HZ), _dt_xy(POSCONTROL_DT_50HZ), diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 2bd8945a84..29f50fc748 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -48,9 +48,7 @@ public: /// Constructor AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const AP_Motors& motors, AC_AttitudeControl& attitude_control, - AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, - AC_P& p_pos_xy); + const AP_Motors& motors, AC_AttitudeControl& attitude_control); // xy_mode - specifies behavior of xy position controller enum xy_mode { @@ -167,9 +165,6 @@ public: float get_leash_down_z() const { return _leash_down_z; } float get_leash_up_z() const { return _leash_up_z; } - /// get_pos_z_kP - returns z position controller's kP gain - float get_pos_z_kP() const { return _p_pos_z.kP(); } - /// /// xy position controller /// @@ -288,10 +283,11 @@ public: // get_leash_xy - returns horizontal leash length in cm float get_leash_xy() const { return _leash; } - /// get_pos_xy_kP - returns xy position controller's kP gain - float get_pos_xy_kP() const { return _p_pos_xy.kP(); } - - /// get horizontal pid controller + /// get pid controllers + AC_P& get_pos_z_p() { return _p_pos_z; } + AC_P& get_vel_z_p() { return _p_vel_z; } + AC_PID& get_accel_z_pid() { return _pid_accel_z; } + AC_P& get_pos_xy_p() { return _p_pos_xy; } AC_PID_2D& get_vel_xy_pid() { return _pid_vel_xy; } /// accessors for reporting @@ -386,14 +382,12 @@ protected: const AP_Motors& _motors; AC_AttitudeControl& _attitude_control; - // references to pid controllers - AC_P& _p_pos_z; - AC_P& _p_vel_z; - AC_PID& _pid_accel_z; - AC_P& _p_pos_xy; - // parameters AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency + AC_P _p_pos_z; + AC_P _p_vel_z; + AC_PID _pid_accel_z; + AC_P _p_pos_xy; AC_PID_2D _pid_vel_xy; // internal variables