diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index c17c75f5f2..0316c5903b 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -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() 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() ); } } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7dd1a9dba8..3408166f5c 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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) } } 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) } } 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), diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 9a37999710..e623abc8b7 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -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"); } diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index a154cd5199..ded477771e 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -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() 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() 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() 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() } 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() // 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() { 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() // 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); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 156ef543b6..a741634248 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -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() { 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() { 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() { #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; } }