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

AC_AutoTune: add option for tuning yaw D-term 

don't touch yaw D or FLTE when tuning the other
make yaw D tune an "axis"
This commit is contained in:
Andy Piper 2023-03-06 09:39:47 +00:00 committed by Andrew Tridgell
parent a86ff90b97
commit 0f6d62c196
4 changed files with 126 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -172,7 +172,9 @@ const char *AC_AutoTune::axis_string() const
case PITCH:
return "Pitch";
case YAW:
return "Yaw";
return "Yaw(E)";
case YAW_D:
return "Yaw(D)";
}
return "";
}
@ -363,6 +365,7 @@ void AC_AutoTune::control_attitude()
start_angle = ahrs_view->pitch_sensor;
break;
case YAW:
case YAW_D:
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
@ -477,6 +480,8 @@ void AC_AutoTune::control_attitude()
axis = PITCH;
} else if (yaw_enabled()) {
axis = YAW;
} else if (yaw_d_enabled()) {
axis = YAW_D;
} else {
complete = true;
}
@ -485,12 +490,22 @@ void AC_AutoTune::control_attitude()
axes_completed |= AUTOTUNE_AXIS_BITMASK_PITCH;
if (yaw_enabled()) {
axis = YAW;
} else if (yaw_d_enabled()) {
axis = YAW_D;
} else {
complete = true;
}
break;
case YAW:
axes_completed |= AUTOTUNE_AXIS_BITMASK_YAW;
if (yaw_d_enabled()) {
axis = YAW_D;
} else {
complete = true;
}
break;
case YAW_D:
axes_completed |= AUTOTUNE_AXIS_BITMASK_YAW_D;
complete = true;
break;
}
@ -512,7 +527,7 @@ void AC_AutoTune::control_attitude()
// reverse direction for multicopter twitch test
positive_direction = twitch_reverse_direction();
if (axis == YAW) {
if (axis == YAW || axis == YAW_D) {
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false);
}
@ -539,6 +554,8 @@ void AC_AutoTune::backup_gains_and_initialise()
axis = PITCH;
} else if (yaw_enabled()) {
axis = YAW;
} else if (yaw_d_enabled()) {
axis = YAW_D;
}
// no axes are complete
axes_completed = 0;
@ -599,10 +616,11 @@ void AC_AutoTune::update_gcs(uint8_t message_id) const
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Pilot Testing");
break;
case AUTOTUNE_MESSAGE_SAVED_GAINS:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s",
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s%s",
(axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"",
(axes_completed&AUTOTUNE_AXIS_BITMASK_PITCH)?"Pitch ":"",
(axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw":"");
(axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw(E)":"",
(axes_completed&AUTOTUNE_AXIS_BITMASK_YAW_D)?"Yaw(D)":"");
break;
}
}
@ -623,6 +641,15 @@ bool AC_AutoTune::yaw_enabled() const
return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_YAW;
}
bool AC_AutoTune::yaw_d_enabled() const
{
#if APM_BUILD_TYPE(APM_BUILD_Heli)
return false;
#else
return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_YAW_D;
#endif
}
/*
check if we have a good position estimate
*/

5
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -26,6 +26,7 @@
#define AUTOTUNE_AXIS_BITMASK_ROLL 1
#define AUTOTUNE_AXIS_BITMASK_PITCH 2
#define AUTOTUNE_AXIS_BITMASK_YAW 4
#define AUTOTUNE_AXIS_BITMASK_YAW_D 8
#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains
@ -69,7 +70,8 @@ protected:
enum AxisType {
ROLL = 0, // roll axis is being tuned (either angle or rate)
PITCH = 1, // pitch axis is being tuned (either angle or rate)
YAW = 2, // pitch axis is being tuned (either angle or rate)
YAW = 2, // yaw axis is being tuned using FLTE (either angle or rate)
YAW_D = 3, // yaw axis is being tuned using D (either angle or rate)
};
//
@ -122,6 +124,7 @@ protected:
bool roll_enabled() const;
bool pitch_enabled() const;
bool yaw_enabled() const;
bool yaw_d_enabled() const;
// update gains for the rate p up tune type
virtual void updating_rate_p_up_all(AxisType test_axis)=0;

22
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -339,6 +339,7 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
tune_accel = tune_pitch_accel;
break;
case YAW:
case YAW_D:
tune_rp = tune_yaw_rp;
tune_rd = tune_yaw_rd;
tune_rff = tune_yaw_rff;
@ -534,6 +535,7 @@ void AC_AutoTune_Heli::load_test_gains()
load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, 0.0f);
break;
case YAW:
case YAW_D:
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
} else {
@ -570,6 +572,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i
attitude_control->set_accel_pitch_max_cdss(max_accel);
break;
case YAW:
case YAW_D:
attitude_control->get_rate_yaw_pid().kP(rate_p);
attitude_control->get_rate_yaw_pid().kI(rate_i);
attitude_control->get_rate_yaw_pid().kD(rate_d);
@ -669,6 +672,7 @@ void AC_AutoTune_Heli::report_final_gains(AxisType test_axis) const
report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel);
break;
case YAW:
case YAW_D:
report_axis_gains("Yaw", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel);
break;
}
@ -793,6 +797,7 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
}
break;
case YAW:
case YAW_D:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
@ -861,6 +866,7 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
}
break;
case YAW:
case YAW_D:
if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
@ -918,6 +924,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi
p_term = attitude_control->get_rate_pitch_pid().get_p();
break;
case YAW:
case YAW_D:
trim_meas_rate = ahrs_view->get_gyro().z;
ff_term = attitude_control->get_rate_yaw_pid().get_ff();
p_term = attitude_control->get_rate_yaw_pid().get_p();
@ -943,8 +950,8 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
float tgt_rate_reading = 0.0f;
float tgt_attitude;
const uint32_t now = AP_HAL::millis();
float target_angle_cd;
float target_rate_cds;
float target_angle_cd = 0.0f;
float target_rate_cds = 0.0f;
float dwell_freq = start_frq;
float target_rate_mag_cds;
const float att_hold_gain = 4.5f;
@ -1057,6 +1064,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
}
break;
case YAW:
case YAW_D:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
@ -1106,6 +1114,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
}
break;
case YAW:
case YAW_D:
command_reading = motors->get_yaw();
if (dwell_type == DRB) {
tgt_rate_reading = (target_angle_cd) / 5730.0f;
@ -1203,6 +1212,7 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
updating_rate_p_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
break;
case YAW:
case YAW_D:
updating_rate_p_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
break;
}
@ -1219,6 +1229,7 @@ void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
updating_rate_d_up(tune_pitch_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
break;
case YAW:
case YAW_D:
updating_rate_d_up(tune_yaw_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
break;
}
@ -1235,6 +1246,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
updating_rate_ff_up(tune_pitch_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt);
break;
case YAW:
case YAW_D:
updating_rate_ff_up(tune_yaw_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt);
// TODO make FF updating routine determine when to set rff gain to zero based on A/C response
if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && counter == AUTOTUNE_SUCCESS_COUNT) {
@ -1257,6 +1269,7 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
updating_angle_p_up(tune_pitch_sp, test_freq, test_gain, test_phase, freq_cnt);
break;
case YAW:
case YAW_D:
updating_angle_p_up(tune_yaw_sp, test_freq, test_gain, test_phase, freq_cnt);
break;
}
@ -1273,6 +1286,7 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd);
break;
case YAW:
case YAW_D:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd);
// rate P and rate D can be non zero for yaw and need to be included in the max allowed gain
if (!is_zero(max_rate_p.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) {
@ -1298,6 +1312,7 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
break;
case YAW:
case YAW_D:
tune_yaw_rd = MAX(0.0f, tune_yaw_rd * AUTOTUNE_RD_BACKOFF);
break;
}
@ -1311,6 +1326,7 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
tune_pitch_rp = MAX(0.0f, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
case YAW_D:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
@ -1324,6 +1340,7 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
break;
case YAW:
case YAW_D:
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
break;
}
@ -1713,6 +1730,7 @@ void AC_AutoTune_Heli::Log_AutoTune()
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
case YAW_D:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max);
break;
}

87
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -51,6 +51,7 @@
#define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value
#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value
#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value
#define AUTOTUNE_FLTE_MIN 2.5f // minimum Rate Yaw error filter value
#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value
#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value
#define AUTOTUNE_SP_MAX 40.0f // maximum Stab P value
@ -81,7 +82,7 @@ const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
// @Param: AXES
// @DisplayName: Autotune axis bitmask
// @Description: 1-byte bitmap of axes to autotune
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:YawD
// @User: Standard
AP_GROUPINFO("AXES", 1, AC_AutoTune_Multi, axis_bitmask, 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT
@ -115,7 +116,7 @@ void AC_AutoTune_Multi::do_gcs_announcements()
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", axis_string(), type_string(), (counter * (100/AUTOTUNE_SUCCESS_COUNT)) );
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", axis_string(), type_string(), (counter * (100/AUTOTUNE_SUCCESS_COUNT)));
announce_time = now;
}
@ -176,7 +177,14 @@ void AC_AutoTune_Multi::backup_gains_and_initialise()
orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
if (yaw_d_enabled() && is_zero(tune_yaw_rd)) {
tune_yaw_rd = min_d;
}
if (yaw_enabled() && is_zero(tune_yaw_rLPF)) {
tune_yaw_rLPF = AUTOTUNE_FLTE_MIN;
}
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
@ -212,7 +220,7 @@ void AC_AutoTune_Multi::load_orig_gains()
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
}
if (yaw_enabled()) {
if (yaw_enabled() || yaw_d_enabled()) {
if (!is_zero(orig_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
@ -255,13 +263,17 @@ void AC_AutoTune_Multi::load_tuned_gains()
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
}
if (yaw_enabled()) {
if (yaw_enabled() || yaw_d_enabled()) {
if (!is_zero(tune_yaw_rp)) {
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);
if (yaw_d_enabled()) {
attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
}
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
@ -293,7 +305,7 @@ void AC_AutoTune_Multi::load_intra_test_gains()
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
}
if (yaw_enabled()) {
if (yaw_enabled() || yaw_d_enabled()) {
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);
@ -329,11 +341,15 @@ void AC_AutoTune_Multi::load_test_gains()
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
break;
case YAW:
case YAW_D:
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().ff(0.0f);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
if (axis == YAW_D) {
attitude_control->get_rate_pitch_pid().kD(tune_yaw_rd);
} else {
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
}
attitude_control->get_rate_yaw_pid().filt_T_hz(0.0f);
attitude_control->get_rate_yaw_pid().slew_limit(0.0f);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
@ -409,15 +425,20 @@ void AC_AutoTune_Multi::save_tuning_gains()
orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
if ((((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled())
|| ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW_D) && yaw_d_enabled())) && !is_zero(tune_yaw_rp)) {
// rate yaw 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().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
if (yaw_d_enabled()) {
attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
}
attitude_control->get_rate_yaw_pid().save_gains();
// stabilize yaw
@ -455,7 +476,10 @@ void AC_AutoTune_Multi::report_final_gains(AxisType test_axis) const
report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel);
break;
case YAW:
report_axis_gains("Yaw", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, 0, tune_yaw_sp, tune_yaw_accel);
report_axis_gains("Yaw(E)", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, 0, tune_yaw_sp, tune_yaw_accel);
break;
case YAW_D:
report_axis_gains("Yaw(D)", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_sp, tune_yaw_accel);
break;
}
}
@ -604,6 +628,9 @@ void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
case YAW:
updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW_D:
updating_rate_p_up_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
}
@ -620,6 +647,9 @@ void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
case YAW:
updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW_D:
updating_rate_d_up(tune_yaw_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
}
@ -636,6 +666,9 @@ void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
case YAW:
updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW_D:
updating_rate_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
}
@ -650,6 +683,7 @@ void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
case YAW_D:
updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
@ -666,6 +700,7 @@ void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
case YAW_D:
updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
@ -691,6 +726,10 @@ void AC_AutoTune_Multi::set_gains_post_tune(AxisType test_axis)
tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
case YAW_D:
tune_yaw_rd = MAX(min_d, tune_yaw_rd * AUTOTUNE_RD_BACKOFF);
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;
case RP_UP:
@ -702,6 +741,7 @@ void AC_AutoTune_Multi::set_gains_post_tune(AxisType test_axis)
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
case YAW_D:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
@ -719,6 +759,7 @@ void AC_AutoTune_Multi::set_gains_post_tune(AxisType test_axis)
tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
break;
case YAW:
case YAW_D:
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
break;
@ -967,6 +1008,9 @@ void AC_AutoTune_Multi::Log_AutoTune()
case YAW:
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
case YAW_D:
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max);
break;
}
} else {
switch (axis) {
@ -979,6 +1023,9 @@ void AC_AutoTune_Multi::Log_AutoTune()
case YAW:
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
case YAW_D:
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max);
break;
}
}
@ -1061,11 +1108,16 @@ void AC_AutoTune_Multi::twitch_test_init()
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
}
case YAW: {
case YAW:
case YAW_D: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
if (axis == YAW_D) {
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz()*2.0f);
} else {
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
}
break;
}
}
@ -1101,7 +1153,8 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, dir_sign * target_angle, 0.0f);
break;
case YAW:
// request pitch to 20deg
case YAW_D:
// request yaw to 20deg
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, dir_sign * target_angle);
break;
}
@ -1120,6 +1173,7 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
attitude_control->rate_bf_pitch_target(dir_sign * target_rate + start_rate);
break;
case YAW:
case YAW_D:
// override body-frame yaw rate
attitude_control->rate_bf_yaw_target(dir_sign * target_rate + start_rate);
break;
@ -1138,6 +1192,7 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
lean_angle = dir_sign * (ahrs_view->pitch_sensor - (int32_t)start_angle);
break;
case YAW:
case YAW_D:
gyro_reading = ahrs_view->get_gyro().z;
lean_angle = dir_sign * wrap_180_cd(ahrs_view->yaw_sensor-(int32_t)start_angle);
break;