Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: move pos-control pids to pos-control library

This commit is contained in:
Randy Mackay 2017-11-21 10:49:11 +09:00
parent 368245017a
commit 8d6f8e4d9c
11 changed files with 37 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/AP_Arming.cpp
View File

@ -256,20 +256,20 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
// Check for 0 value PID's - some items can / should be 0 and as such are not checked.
// If the ATC_RAT_*_FF is non zero then the corresponding ATC_RAT_* PIDS can be 0.
if (is_zero(copter.g.p_pos_xy.kP())) {
parameter_checks_pid_warning_message(display_failure, "POS_XY_P");
if (is_zero(copter.pos_control->get_pos_xy_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_POSXY_P");
return false;
} else if (is_zero(copter.g.p_alt_hold.kP())) {
parameter_checks_pid_warning_message(display_failure, "POS_Z_P");
} else if (is_zero(copter.pos_control->get_pos_z_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_POSZ_P");
return false;
} else if (is_zero(copter.g.p_vel_z.kP())) {
parameter_checks_pid_warning_message(display_failure, "VEL_Z_P");
} else if (is_zero(copter.pos_control->get_vel_z_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_VELZ_P");
return false;
} else if (is_zero(copter.g.pid_accel_z.kP())) {
parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_P");
} else if (is_zero(copter.pos_control->get_accel_z_pid().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_ACCELZ_P");
return false;
} else if (is_zero(copter.g.pid_accel_z.kI())) {
parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_I");
} else if (is_zero(copter.pos_control->get_accel_z_pid().kI())) {
parameter_checks_pid_warning_message(display_failure, "PSC_ACCELZ_I");
return false;
} else if (is_zero(copter.attitude_control->get_rate_roll_pid().kP()) && is_zero(copter.attitude_control->get_rate_roll_pid().ff())) {
parameter_checks_pid_warning_message(display_failure, "ATC_RAT_RLL_P");

4
ArduCopter/Attitude.cpp
View File

@ -283,7 +283,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
float Copter::get_avoidance_adjusted_climbrate(float target_rate)
{
#if AC_AVOID_ENABLED == ENABLED
avoid.adjust_velocity_z(pos_control->get_pos_z_kP(), pos_control->get_accel_z(), target_rate, G_Dt);
avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);
return target_rate;
#else
return target_rate;
@ -296,7 +296,7 @@ void Copter::set_accel_throttle_I_from_pilot_throttle()
// get last throttle input sent to attitude controller
float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0f, 1.0f);
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
pos_control->get_accel_z_pid().set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
}
// rotate vector from vehicle's perspective to North-East frame

2
ArduCopter/GCS_Mavlink.cpp
View File

@ -253,7 +253,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 8) {
const DataFlash_Class::PID_Info &pid_info = g.pid_accel_z.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = copter.pos_control->get_accel_z_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
pid_info.desired*0.01f,
-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS),

2
ArduCopter/Log.cpp
View File

@ -231,7 +231,7 @@ void Copter::Log_Write_Attitude()
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() );
}
}

63
ArduCopter/Parameters.cpp
View File

@ -555,61 +555,6 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Advanced
GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT),
// @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
// @Range: 1.000 8.000
// @User: Standard
GGROUP(p_vel_z, "VEL_Z_", AC_P),
// @Param: ACCEL_Z_P
// @DisplayName: Throttle acceleration controller P gain
// @Description: Throttle acceleration 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: ACCEL_Z_I
// @DisplayName: Throttle acceleration controller I gain
// @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration
// @Range: 0.000 3.000
// @User: Standard
// @Param: ACCEL_Z_IMAX
// @DisplayName: Throttle acceleration controller I gain maximum
// @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate
// @Range: 0 1000
// @Units: d%
// @User: Standard
// @Param: ACCEL_Z_D
// @DisplayName: Throttle acceleration controller D gain
// @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration
// @Range: 0.000 0.400
// @User: Standard
// @Param: ACCEL_Z_FILT
// @DisplayName: Throttle acceleration 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
GGROUP(pid_accel_z, "ACCEL_Z_", AC_PID),
// @Param: POS_Z_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
GGROUP(p_alt_hold, "POS_Z_", AC_P),
// @Param: POS_XY_P
// @DisplayName: Position (horizonal) controller P gain
// @Description: Loiter 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
GGROUP(p_pos_xy, "POS_XY_", AC_P),
// variables not in the g class which contain EEPROM saved variables
#if CAMERA == ENABLED
@ -1124,6 +1069,14 @@ void Copter::convert_pid_parameters(void)
{ 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" },
{ Parameters::k_param_p_vel_z, 0, AP_PARAM_FLOAT, "PSC_VELZ_P" },
{ Parameters::k_param_pid_accel_z, 0, AP_PARAM_FLOAT, "PSC_ACCELZ_P" },
{ Parameters::k_param_pid_accel_z, 1, AP_PARAM_FLOAT, "PSC_ACCELZ_I" },
{ Parameters::k_param_pid_accel_z, 2, AP_PARAM_FLOAT, "PSC_ACCELZ_D" },
{ Parameters::k_param_pid_accel_z, 5, AP_PARAM_FLOAT, "PSC_ACCELZ_IMAX" },
{ Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCELZ_FILT" },
{ Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" },
{ Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" },
};
const AP_Param::ConversionInfo throttle_conversion_info[] = {
{ Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" },

27
ArduCopter/Parameters.h
View File

@ -340,19 +340,19 @@ public:
k_param_p_stabilize_roll, // remove
k_param_p_stabilize_pitch, // remove
k_param_p_stabilize_yaw, // remove
k_param_p_pos_xy,
k_param_p_pos_xy, // remove
k_param_p_loiter_lon, // remove
k_param_pid_loiter_rate_lat, // remove
k_param_pid_loiter_rate_lon, // remove
k_param_pid_nav_lat, // remove
k_param_pid_nav_lon, // remove
k_param_p_alt_hold,
k_param_p_vel_z,
k_param_p_alt_hold, // remove
k_param_p_vel_z, // remove
k_param_pid_optflow_roll, // remove
k_param_pid_optflow_pitch, // remove
k_param_acro_balance_roll_old, // remove
k_param_acro_balance_pitch_old, // remove
k_param_pid_accel_z,
k_param_pid_accel_z, // remove
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_acro_yaw_p,
@ -468,13 +468,6 @@ public:
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
// PI/D controllers
AC_P p_vel_z;
AC_PID pid_accel_z;
AC_P p_pos_xy;
AC_P p_alt_hold;
// Autotune
AP_Int8 autotune_axis_bitmask;
AP_Float autotune_aggressiveness;
@ -482,17 +475,7 @@ public:
// Note: keep initializers here in the same order as they are declared
// above.
Parameters() :
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
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),
// P controller initial P
//----------------------------------------------------------------------
p_pos_xy (POS_XY_P),
p_alt_hold (ALT_HOLD_P)
Parameters()
{
}
};

35
ArduCopter/config.h
View File

@ -460,13 +460,6 @@
# define ANGLE_RATE_MAX 18000 // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter position control gains
//
#ifndef POS_XY_P
# define POS_XY_P 1.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// Stop mode defaults
//
@ -488,39 +481,13 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
// Throttle control defaults
//
#ifndef THR_DZ_DEFAULT
# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter
#endif
#ifndef ALT_HOLD_P
# define ALT_HOLD_P 1.0f
#endif
// Velocity (vertical) control gains
#ifndef VEL_Z_P
# define VEL_Z_P 5.0f
#endif
// Accel (vertical) control gains
#ifndef ACCEL_Z_P
# define ACCEL_Z_P 0.50f
#endif
#ifndef ACCEL_Z_I
# define ACCEL_Z_I 1.00f
#endif
#ifndef ACCEL_Z_D
# define ACCEL_Z_D 0.0f
#endif
#ifndef ACCEL_Z_IMAX
# define ACCEL_Z_IMAX 800
#endif
#ifndef ACCEL_Z_FILT_HZ
# define ACCEL_Z_FILT_HZ 20.0f
#endif
// default maximum vertical velocity and acceleration the pilot may request
#ifndef PILOT_VELZ_MAX
# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s

2
ArduCopter/mode_guided.cpp
View File

@ -653,7 +653,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
#if AC_AVOID_ENABLED
// limit the velocity to prevent fence violations
_copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
_copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
// get avoidance adjusted climb rate
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
#endif

2
ArduCopter/mode_land.cpp
View File

@ -184,7 +184,7 @@ void Copter::land_run_vertical_control(bool pause_descent)
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
// Compute a vertical velocity demand such that the vehicle approaches LAND_START_ALT. Without the below constraint, this would cause the vehicle to hover at LAND_START_ALT.
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control->get_accel_z(), G_Dt);
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), G_Dt);
// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));

4
ArduCopter/system.cpp
View File

@ -597,9 +597,7 @@ void Copter::allocate_motors(void)
}
AP_Param::load_object_from_eeprom(attitude_control, ac_var_info);
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);
pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
if (pos_control == nullptr) {
AP_HAL::panic("Unable to allocate PosControl");
}

12
ArduCopter/tuning.cpp
View File

@ -63,28 +63,28 @@ void Copter::tuning() {
// Altitude and throttle tuning
case TUNING_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
pos_control->get_pos_z_p().kP(tuning_value);
break;
case TUNING_THROTTLE_RATE_KP:
g.p_vel_z.kP(tuning_value);
pos_control->get_vel_z_p().kP(tuning_value);
break;
case TUNING_ACCEL_Z_KP:
g.pid_accel_z.kP(tuning_value);
pos_control->get_accel_z_pid().kP(tuning_value);
break;
case TUNING_ACCEL_Z_KI:
g.pid_accel_z.kI(tuning_value);
pos_control->get_accel_z_pid().kI(tuning_value);
break;
case TUNING_ACCEL_Z_KD:
g.pid_accel_z.kD(tuning_value);
pos_control->get_accel_z_pid().kD(tuning_value);
break;
// Loiter and navigation tuning
case TUNING_LOITER_POSITION_KP:
g.p_pos_xy.kP(tuning_value);
pos_control->get_pos_xy_p().kP(tuning_value);
break;
case TUNING_VEL_XY_KP: