aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Randy Mackay 2018-01-15 20:58:57 +09:00
parent 8d6f8e4d9c
commit 6a701b2412
5 changed files with 9 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/ArduSub.cpp
View File

@ -264,7 +264,7 @@ void Sub::ten_hz_logging_loop()
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());
}
}
if (should_log(MASK_LOG_MOTBATT)) {
@ -298,7 +298,7 @@ void Sub::twentyfive_hz_logging()
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());
}
}

2
ArduSub/GCS_Mavlink.cpp
View File

@ -451,7 +451,7 @@ void Sub::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 = 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),

55
ArduSub/Parameters.cpp
View File

@ -451,61 +451,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Advanced
GSCALAR(acro_expo, "ACRO_EXPO", ACRO_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

29
ArduSub/Parameters.h
View File

@ -149,11 +149,11 @@ public:
// PID Controllers
k_param_p_pos_xy = 126,
k_param_p_alt_hold,
k_param_p_pos_xy = 126, // deprecated
k_param_p_alt_hold, // deprecated
k_param_pi_vel_xy, // deprecated
k_param_p_vel_z,
k_param_pid_accel_z,
k_param_p_vel_z, // deprecated
k_param_pid_accel_z, // deprecated
// Failsafes
@ -304,31 +304,12 @@ public:
AP_Int8 acro_trainer;
AP_Float acro_expo;
// PI/D controllers
AC_P p_vel_z;
AC_PID pid_accel_z;
AC_P p_pos_xy;
AC_P p_alt_hold;
AP_Float surface_depth;
AP_Int8 frame_configuration;
// 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, MAIN_LOOP_SECONDS),
// P controller initial P
//----------------------------------------------------------------------
p_pos_xy(POS_XY_P),
p_alt_hold(ALT_HOLD_P)
Parameters()
{
}
};

3
ArduSub/Sub.cpp
View File

@ -48,8 +48,7 @@ Sub::Sub(void)
inertial_nav(ahrs),
ahrs_view(ahrs, ROTATION_NONE),
attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS),
pos_control(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(ahrs_view, inertial_nav, motors, attitude_control),
wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
circle_nav(inertial_nav, ahrs_view, pos_control),
pmTest1(0),