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

AC_AutoTune: Multi and Heli: only load tuned gains if a axis has completed successfully

This commit is contained in:
Iampete1 2024-08-04 23:36:11 +01:00 committed by Randy Mackay
parent 9da6f321a0
commit 94d3e2673a
2 changed files with 33 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -487,16 +487,14 @@ void AC_AutoTune_Heli::load_tuned_gains()
attitude_control->set_accel_roll_max_cdss(0.0f);
attitude_control->set_accel_pitch_max_cdss(0.0f);
}
if (roll_enabled()) {
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) {
load_gain_set(AxisType::ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate);
}
if (pitch_enabled()) {
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) {
load_gain_set(AxisType::PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate);
}
if (yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate);
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate);
}
}

63
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -260,43 +260,38 @@ void AC_AutoTune_Multi::load_tuned_gains()
attitude_control->set_accel_roll_max_cdss(0.0);
attitude_control->set_accel_pitch_max_cdss(0.0);
}
if (roll_enabled()) {
if (!is_zero(tune_roll_rp)) {
attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
attitude_control->get_angle_roll_p().set_kP(tune_roll_sp);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) {
attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
attitude_control->get_angle_roll_p().set_kP(tune_roll_sp);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
if (pitch_enabled()) {
if (!is_zero(tune_pitch_rp)) {
attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
if (yaw_enabled() || yaw_d_enabled()) {
if (!is_zero(tune_yaw_rp)) {
attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
if (yaw_d_enabled()) {
attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF);
}
attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
if ((((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled())
|| ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW_D) && yaw_d_enabled())) && !is_zero(tune_yaw_rp)) {
attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
if (yaw_d_enabled()) {
attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF);
}
attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
}