mirror of https://github.com/ArduPilot/ardupilot
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:
parent
a86ff90b97
commit
0f6d62c196
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
|
||||
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_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);
|
||||
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);
|
||||
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);
|
||||
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;
|
||||
|
|
Loading…
Reference in New Issue