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

Copter: access angle and rate PIDs through attitude controller

mission-4.1.18
Randy Mackay 9 years ago
parent
bde498375c
commit
1f20a5ef69
5 changed files with 139 additions and 139 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. 12
      ArduCopter/ArduCopter.cpp
  2. 6
      ArduCopter/GCS_Mavlink.cpp
  3. 2
      ArduCopter/arming_checks.cpp
  4. 216
      ArduCopter/control_autotune.cpp
  5. 42
      ArduCopter/tuning.cpp

12
ArduCopter/ArduCopter.cpp
Unescape View File

@ -383,9 +383,9 @@ void Copter::ten_hz_logging_loop() @@ -383,9 +383,9 @@ void Copter::ten_hz_logging_loop()
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
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() );
}
}
@ -426,9 +426,9 @@ void Copter::fifty_hz_logging_loop() @@ -426,9 +426,9 @@ void Copter::fifty_hz_logging_loop()
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
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() );
}
}

6
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -439,7 +439,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) @@ -439,7 +439,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
{
const Vector3f &gyro = ahrs.get_gyro();
if (g.gcs_pid_mask & 1) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_roll.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
pid_info.desired*0.01f,
degrees(gyro.x),
@ -452,7 +452,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) @@ -452,7 +452,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 2) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
pid_info.desired*0.01f,
degrees(gyro.y),
@ -465,7 +465,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan) @@ -465,7 +465,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 4) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
pid_info.desired*0.01f,
degrees(gyro.z),

2
ArduCopter/arming_checks.cpp
Unescape View File

@ -310,7 +310,7 @@ bool Copter::pre_arm_checks(bool display_failure) @@ -310,7 +310,7 @@ bool Copter::pre_arm_checks(bool display_failure)
}
// acro balance parameter check
if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) {
if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}

216
ArduCopter/control_autotune.cpp
Unescape View File

@ -393,7 +393,7 @@ void Copter::autotune_attitude_control() @@ -393,7 +393,7 @@ void Copter::autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_roll(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
autotune_start_angle = ahrs.roll_sensor;
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_roll.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_roll_pid().filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -405,7 +405,7 @@ void Copter::autotune_attitude_control() @@ -405,7 +405,7 @@ void Copter::autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_pitch(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
autotune_start_angle = ahrs.pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_pitch.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_pitch_pid().filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -767,35 +767,35 @@ void Copter::autotune_backup_gains_and_initialise() @@ -767,35 +767,35 @@ void Copter::autotune_backup_gains_and_initialise()
orig_bf_feedforward = attitude_control.get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.p_stabilize_roll.kP();
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
orig_roll_accel = attitude_control.get_accel_roll_max();
tune_roll_rp = g.pid_rate_roll.kP();
tune_roll_rd = g.pid_rate_roll.kD();
tune_roll_sp = g.p_stabilize_roll.kP();
tune_roll_rp = attitude_control.get_rate_roll_pid().kP();
tune_roll_rd = attitude_control.get_rate_roll_pid().kD();
tune_roll_sp = attitude_control.get_angle_roll_p().kP();
tune_roll_accel = attitude_control.get_accel_roll_max();
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.p_stabilize_pitch.kP();
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control.get_accel_pitch_max();
tune_pitch_rp = g.pid_rate_pitch.kP();
tune_pitch_rd = g.pid_rate_pitch.kD();
tune_pitch_sp = g.p_stabilize_pitch.kP();
tune_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
tune_pitch_sp = attitude_control.get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control.get_accel_pitch_max();
orig_yaw_rp = g.pid_rate_yaw.kP();
orig_yaw_ri = g.pid_rate_yaw.kI();
orig_yaw_rd = g.pid_rate_yaw.kD();
orig_yaw_rLPF = g.pid_rate_yaw.filt_hz();
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
orig_yaw_accel = attitude_control.get_accel_yaw_max();
orig_yaw_sp = g.p_stabilize_yaw.kP();
tune_yaw_rp = g.pid_rate_yaw.kP();
tune_yaw_rLPF = g.pid_rate_yaw.filt_hz();
tune_yaw_sp = g.p_stabilize_yaw.kP();
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
tune_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
tune_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
tune_yaw_sp = attitude_control.get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control.get_accel_yaw_max();
Log_Write_Event(DATA_AUTOTUNE_INITIALISED);
@ -808,29 +808,29 @@ void Copter::autotune_load_orig_gains() @@ -808,29 +808,29 @@ void Copter::autotune_load_orig_gains()
attitude_control.bf_feedforward(orig_bf_feedforward);
if (autotune_roll_enabled()) {
if (!is_zero(orig_roll_rp)) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_ri);
g.pid_rate_roll.kD(orig_roll_rd);
g.p_stabilize_roll.kP(orig_roll_sp);
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
attitude_control.get_rate_roll_pid().kI(orig_roll_ri);
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
attitude_control.set_accel_roll_max(orig_roll_accel);
}
}
if (autotune_pitch_enabled()) {
if (!is_zero(orig_pitch_rp)) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.p_stabilize_pitch.kP(orig_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control.set_accel_pitch_max(orig_pitch_accel);
}
}
if (autotune_yaw_enabled()) {
if (!is_zero(orig_yaw_rp)) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_ri);
g.pid_rate_yaw.kD(orig_yaw_rd);
g.pid_rate_yaw.filt_hz(orig_yaw_rLPF);
g.p_stabilize_yaw.kP(orig_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(orig_yaw_ri);
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control.set_accel_yaw_max(orig_yaw_accel);
}
}
@ -846,29 +846,29 @@ void Copter::autotune_load_tuned_gains() @@ -846,29 +846,29 @@ void Copter::autotune_load_tuned_gains()
}
if (autotune_roll_enabled()) {
if (!is_zero(tune_roll_rp)) {
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.p_stabilize_roll.kP(tune_roll_sp);
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
attitude_control.set_accel_roll_max(tune_roll_accel);
}
}
if (autotune_pitch_enabled()) {
if (!is_zero(tune_pitch_rp)) {
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.p_stabilize_pitch.kP(tune_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control.set_accel_pitch_max(tune_pitch_accel);
}
}
if (autotune_yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.p_stabilize_yaw.kP(tune_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control.set_accel_yaw_max(tune_yaw_accel);
}
}
@ -882,23 +882,23 @@ void Copter::autotune_load_intra_test_gains() @@ -882,23 +882,23 @@ void Copter::autotune_load_intra_test_gains()
// sanity check the gains
attitude_control.bf_feedforward(true);
if (autotune_roll_enabled()) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_roll.kD(orig_roll_rd);
g.p_stabilize_roll.kP(orig_roll_sp);
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
attitude_control.get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
}
if (autotune_pitch_enabled()) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.p_stabilize_pitch.kP(orig_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
}
if (autotune_yaw_enabled()) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_yaw.kD(orig_yaw_rd);
g.pid_rate_yaw.filt_hz(orig_yaw_rLPF);
g.p_stabilize_yaw.kP(orig_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
}
}
@ -908,23 +908,23 @@ void Copter::autotune_load_twitch_gains() @@ -908,23 +908,23 @@ void Copter::autotune_load_twitch_gains()
{
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*0.01f);
g.pid_rate_roll.kD(tune_roll_rd);
g.p_stabilize_roll.kP(tune_roll_sp);
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*0.01f);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
break;
case AUTOTUNE_AXIS_PITCH:
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*0.01f);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.p_stabilize_pitch.kP(tune_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
break;
case AUTOTUNE_AXIS_YAW:
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*0.01f);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.p_stabilize_yaw.kP(tune_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
break;
}
}
@ -945,67 +945,67 @@ void Copter::autotune_save_tuning_gains() @@ -945,67 +945,67 @@ void Copter::autotune_save_tuning_gains()
// sanity check the rate P values
if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) {
// rate roll gains
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.pid_rate_roll.save_gains();
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_rate_roll_pid().save_gains();
// stabilize roll
g.p_stabilize_roll.kP(tune_roll_sp);
g.p_stabilize_roll.save_gains();
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
attitude_control.get_angle_roll_p().save_gains();
// acceleration roll
attitude_control.save_accel_roll_max(tune_roll_accel);
// resave pids to originals in case the autotune is run again
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.p_stabilize_roll.kP();
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
}
if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) {
// rate pitch gains
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pid_rate_pitch.save_gains();
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_rate_pitch_pid().save_gains();
// stabilize pitch
g.p_stabilize_pitch.kP(tune_pitch_sp);
g.p_stabilize_pitch.save_gains();
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control.get_angle_pitch_p().save_gains();
// acceleration pitch
attitude_control.save_accel_pitch_max(tune_pitch_accel);
// resave pids to originals in case the autotune is run again
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.p_stabilize_pitch.kP();
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
}
if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) {
// rate yaw gains
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.pid_rate_yaw.save_gains();
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_rate_yaw_pid().save_gains();
// stabilize yaw
g.p_stabilize_yaw.kP(tune_yaw_sp);
g.p_stabilize_yaw.save_gains();
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control.get_angle_yaw_p().save_gains();
// acceleration yaw
attitude_control.save_accel_yaw_max(tune_yaw_accel);
// resave pids to originals in case the autotune is run again
orig_yaw_rp = g.pid_rate_yaw.kP();
orig_yaw_ri = g.pid_rate_yaw.kI();
orig_yaw_rd = g.pid_rate_yaw.kD();
orig_yaw_rLPF = g.pid_rate_yaw.filt_hz();
orig_yaw_sp = g.p_stabilize_yaw.kP();
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
}
// update GCS and log save gains event
autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);

42
ArduCopter/tuning.cpp
Unescape View File

@ -28,36 +28,36 @@ void Copter::tuning() { @@ -28,36 +28,36 @@ void Copter::tuning() {
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
attitude_control.get_angle_roll_p().kP(tuning_value);
attitude_control.get_angle_pitch_p().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
attitude_control.get_rate_roll_pid().kP(tuning_value);
attitude_control.get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
attitude_control.get_rate_roll_pid().kI(tuning_value);
attitude_control.get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
attitude_control.get_rate_roll_pid().kD(tuning_value);
attitude_control.get_rate_pitch_pid().kD(tuning_value);
break;
// Yaw tuning
case TUNING_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
attitude_control.get_angle_yaw_p().kP(tuning_value);
break;
case TUNING_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
attitude_control.get_rate_yaw_pid().kP(tuning_value);
break;
case TUNING_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
attitude_control.get_rate_yaw_pid().kD(tuning_value);
break;
// Altitude and throttle tuning
@ -115,15 +115,15 @@ void Copter::tuning() { @@ -115,15 +115,15 @@ void Copter::tuning() {
break;
case TUNING_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
attitude_control.get_heli_rate_pitch_pid().ff(tuning_value);
break;
case TUNING_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
attitude_control.get_heli_rate_roll_pid().ff(tuning_value);
break;
case TUNING_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
attitude_control.get_heli_rate_yaw_pid().ff(tuning_value);
break;
#endif
@ -179,27 +179,27 @@ void Copter::tuning() { @@ -179,27 +179,27 @@ void Copter::tuning() {
break;
case TUNING_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
attitude_control.get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
attitude_control.get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
attitude_control.get_rate_pitch_pid().kD(tuning_value);
break;
case TUNING_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
attitude_control.get_rate_roll_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
attitude_control.get_rate_roll_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
attitude_control.get_rate_roll_pid().kD(tuning_value);
break;
#if FRAME_CONFIG != HELI_FRAME
@ -209,7 +209,7 @@ void Copter::tuning() { @@ -209,7 +209,7 @@ void Copter::tuning() {
#endif
case TUNING_RATE_YAW_FILT:
g.pid_rate_yaw.filt_hz(tuning_value);
attitude_control.get_rate_yaw_pid().filt_hz(tuning_value);
break;
}
}

Write Preview
Loading…
Cancel
Save