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

Copter: access angle and rate PIDs through attitude controller

This commit is contained in:
Randy Mackay 2016-02-15 15:27:29 +09:00
parent bde498375c
commit 1f20a5ef69
5 changed files with 139 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/ArduCopter.cpp
View File

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

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -439,7 +439,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
{ {
const Vector3f &gyro = ahrs.get_gyro(); const Vector3f &gyro = ahrs.get_gyro();
if (g.gcs_pid_mask & 1) { 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, mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
pid_info.desired*0.01f, pid_info.desired*0.01f,
degrees(gyro.x), degrees(gyro.x),
@ -452,7 +452,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
} }
} }
if (g.gcs_pid_mask & 2) { 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, mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
pid_info.desired*0.01f, pid_info.desired*0.01f,
degrees(gyro.y), degrees(gyro.y),
@ -465,7 +465,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
} }
} }
if (g.gcs_pid_mask & 4) { 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, mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
pid_info.desired*0.01f, pid_info.desired*0.01f,
degrees(gyro.z), degrees(gyro.z),

2
ArduCopter/arming_checks.cpp
View File

@ -310,7 +310,7 @@ bool Copter::pre_arm_checks(bool display_failure)
} }
// acro balance parameter check // 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) { if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
} }

216
ArduCopter/control_autotune.cpp
View File

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

42
ArduCopter/tuning.cpp
View File

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