zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_PosControl: move pids to be local

mission-4.1.18
Randy Mackay 7 years ago
parent
017e71a748
commit
c70d3e0ab8
2 changed files with 89 additions and 25 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 88
      libraries/AC_AttitudeControl/AC_PosControl.cpp
  2. 26
      libraries/AC_AttitudeControl/AC_PosControl.h

88
libraries/AC_AttitudeControl/AC_PosControl.cpp
Unescape View File

@ -6,6 +6,15 @@ extern const AP_HAL::HAL& hal; @@ -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; @@ -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[] = { @@ -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[] = { @@ -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[] = { @@ -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),

26
libraries/AC_AttitudeControl/AC_PosControl.h
Unescape View File

@ -48,9 +48,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save