Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: move pids to be local

Browse Source
This commit is contained in:
Randy Mackay 2018-01-15 19:52:55 +09:00
parent 017e71a748
commit c70d3e0ab8
2 changed files with 89 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -6,6 +6,15 @@ extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane // 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_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_I 0.35f // horizontal velocity controller I gain default
# define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D 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 # define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
#else #else
// default gains for Copter and Sub // 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_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_I 0.5f // horizontal velocity controller I gain default
# define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D 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 // @User: Advanced
AP_GROUPINFO("_ACC_XY_FILT", 1, AC_PosControl, _accel_xy_filt_hz, POSCONTROL_ACCEL_FILTER_HZ), 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 // @Param: _VELXY_P
// @DisplayName: Velocity (horizontal) P gain // @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration // @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 // @Range: 0 100
// @Units: Hz // @Units: Hz
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 6, AC_PosControl, AC_PID_2D),
AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 2, AC_PosControl, AC_PID_2D),
AP_GROUPEND AP_GROUPEND
}; };
@ -87,17 +159,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// their values. // their values.
// //
AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control, 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) :
_ahrs(ahrs), _ahrs(ahrs),
_inav(inav), _inav(inav),
_motors(motors), _motors(motors),
_attitude_control(attitude_control), _attitude_control(attitude_control),
_p_pos_z(p_pos_z), _p_pos_z(POSCONTROL_POS_Z_P),
_p_vel_z(p_vel_z), _p_vel_z(POSCONTROL_VEL_Z_P),
_pid_accel_z(pid_accel_z), _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(p_pos_xy), _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), _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(POSCONTROL_DT_400HZ),
_dt_xy(POSCONTROL_DT_50HZ), _dt_xy(POSCONTROL_DT_50HZ),

26
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -48,9 +48,7 @@ public:
/// Constructor /// Constructor
AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control, 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);
// xy_mode - specifies behavior of xy position controller // xy_mode - specifies behavior of xy position controller
enum xy_mode { enum xy_mode {
@ -167,9 +165,6 @@ public:
float get_leash_down_z() const { return _leash_down_z; } float get_leash_down_z() const { return _leash_down_z; }
float get_leash_up_z() const { return _leash_up_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 /// xy position controller
/// ///
@ -288,10 +283,11 @@ public:
// get_leash_xy - returns horizontal leash length in cm // get_leash_xy - returns horizontal leash length in cm
float get_leash_xy() const { return _leash; } float get_leash_xy() const { return _leash; }
/// get_pos_xy_kP - returns xy position controller's kP gain /// get pid controllers
float get_pos_xy_kP() const { return _p_pos_xy.kP(); } AC_P& get_pos_z_p() { return _p_pos_z; }
AC_P& get_vel_z_p() { return _p_vel_z; }
/// get horizontal pid controller 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; } AC_PID_2D& get_vel_xy_pid() { return _pid_vel_xy; }
/// accessors for reporting /// accessors for reporting
@ -386,14 +382,12 @@ protected:
const AP_Motors& _motors; const AP_Motors& _motors;
AC_AttitudeControl& _attitude_control; 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 // parameters
AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency 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; AC_PID_2D _pid_vel_xy;
// internal variables // internal variables