Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: improve RFF dwell test and data processing

Browse Source
This commit is contained in:
bnsgeyer 2024-05-19 22:36:38 -04:00 committed by Bill Geyer
parent 30c2e1f53b
commit ad1b01cd56
3 changed files with 100 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -283,9 +283,9 @@ protected:
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_dff, orig_pitch_fltt, orig_pitch_smax, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_dff, orig_yaw_fltt, orig_yaw_smax, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel, orig_roll_rate;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_dff, orig_pitch_fltt, orig_pitch_smax, orig_pitch_sp, orig_pitch_accel, orig_pitch_rate;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_dff, orig_yaw_fltt, orig_yaw_smax, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel, orig_yaw_rate;
bool orig_bf_feedforward;
// currently being tuned parameter values

129
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -142,6 +142,8 @@ void AC_AutoTune_Heli::test_init()
AC_AutoTune_FreqResp::ResponseType resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
FreqRespCalcType calc_type = RATE;
FreqRespInput freq_resp_input = TARGET;
float freq_resp_amplitude = 5.0f; // amplitude in deg
float filter_freq = 10.0f;
switch (tune_type) {
case RFF_UP:
if (!is_positive(next_test_freq)) {
@ -150,10 +152,12 @@ void AC_AutoTune_Heli::test_init()
start_freq = next_test_freq;
}
stop_freq = start_freq;
filter_freq = start_freq;
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
// attitude_control->use_sqrt_controller(true); // invoke rate and acceleration limits to provide square wave rate response.
// freq_resp_amplitude = 10.0f; // increase amplitude with limited rate to get desired square wave rate response
// variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
calc_type = RATE;
@ -172,6 +176,7 @@ void AC_AutoTune_Heli::test_init()
stop_freq = start_freq;
test_accel_max = 0.0f;
}
filter_freq = start_freq;
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
@ -201,6 +206,7 @@ void AC_AutoTune_Heli::test_init()
start_freq = next_test_freq;
}
stop_freq = start_freq;
filter_freq = start_freq;
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
@ -223,6 +229,7 @@ void AC_AutoTune_Heli::test_init()
stop_freq = start_freq;
test_accel_max = 0.0f;
}
filter_freq = start_freq;
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
@ -238,6 +245,7 @@ void AC_AutoTune_Heli::test_init()
start_freq = min_sweep_freq;
stop_freq = max_sweep_freq;
test_accel_max = 0.0f;
filter_freq = start_freq;
// variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE;
@ -256,7 +264,7 @@ void AC_AutoTune_Heli::test_init()
// initialize dwell test method
dwell_test_init(start_freq, stop_freq, start_freq, freq_resp_input, calc_type, resp_type, input_type);
dwell_test_init(start_freq, stop_freq, freq_resp_amplitude, filter_freq, freq_resp_input, calc_type, resp_type, input_type);
start_angles = Vector3f(roll_cd, pitch_cd, desired_yaw_cd); // heli specific
}
@ -418,6 +426,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
orig_roll_smax = attitude_control->get_rate_roll_pid().slew_limit();
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max_cdss();
orig_roll_rate = attitude_control->get_ang_vel_roll_max_degs();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
tune_roll_rff = attitude_control->get_rate_roll_pid().ff();
@ -432,6 +441,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
orig_pitch_smax = attitude_control->get_rate_pitch_pid().slew_limit();
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
orig_pitch_rate = attitude_control->get_ang_vel_pitch_max_degs();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
tune_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
@ -447,6 +457,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
orig_yaw_rate = attitude_control->get_ang_vel_yaw_max_degs();
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
tune_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
@ -463,13 +474,13 @@ void AC_AutoTune_Heli::load_orig_gains()
{
attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) {
load_gain_set(ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax);
load_gain_set(ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate);
}
if (pitch_enabled()) {
load_gain_set(PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax);
load_gain_set(PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate);
}
if (yaw_enabled()) {
load_gain_set(YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax);
load_gain_set(YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate);
}
}
@ -482,14 +493,14 @@ void AC_AutoTune_Heli::load_tuned_gains()
attitude_control->set_accel_pitch_max_cdss(0.0f);
}
if (roll_enabled()) {
load_gain_set(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);
load_gain_set(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()) {
load_gain_set(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);
load_gain_set(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(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);
load_gain_set(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);
}
}
}
@ -502,13 +513,13 @@ void AC_AutoTune_Heli::load_intra_test_gains()
// sanity check the gains
attitude_control->bf_feedforward(true);
if (roll_enabled()) {
load_gain_set(ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax);
load_gain_set(ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate);
}
if (pitch_enabled()) {
load_gain_set(PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax);
load_gain_set(PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate);
}
if (yaw_enabled()) {
load_gain_set(YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax);
load_gain_set(YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate);
}
}
@ -516,7 +527,7 @@ void AC_AutoTune_Heli::load_intra_test_gains()
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli::load_test_gains()
{
float rate_p, rate_i, rate_d;
float rate_p, rate_i, rate_d, rate_test_max;
switch (axis) {
case ROLL:
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
@ -532,7 +543,14 @@ void AC_AutoTune_Heli::load_test_gains()
rate_p = tune_roll_rp;
rate_d = tune_roll_rd;
}
load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f);
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_roll_rate;
}
*/
rate_test_max = orig_roll_rate;
load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f, rate_test_max);
break;
case PITCH:
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
@ -548,7 +566,14 @@ void AC_AutoTune_Heli::load_test_gains()
rate_p = tune_pitch_rp;
rate_d = tune_pitch_rd;
}
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);
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_pitch_rate;
}
*/
rate_test_max = orig_pitch_rate;
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, rate_test_max);
break;
case YAW:
case YAW_D:
@ -558,13 +583,20 @@ void AC_AutoTune_Heli::load_test_gains()
// freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_yaw_ri;
}
load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f);
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_yaw_rate;
}
*/
rate_test_max = orig_yaw_rate;
load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f, rate_test_max);
break;
}
}
// load gains
void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax)
void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax, float max_rate)
{
switch (s_axis) {
case ROLL:
@ -576,6 +608,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i
attitude_control->get_rate_roll_pid().slew_limit(smax);
attitude_control->get_angle_roll_p().kP(angle_p);
attitude_control->set_accel_roll_max_cdss(max_accel);
attitude_control->set_ang_vel_roll_max_degs(max_rate);
break;
case PITCH:
attitude_control->get_rate_pitch_pid().kP(rate_p);
@ -586,6 +619,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i
attitude_control->get_rate_pitch_pid().slew_limit(smax);
attitude_control->get_angle_pitch_p().kP(angle_p);
attitude_control->set_accel_pitch_max_cdss(max_accel);
attitude_control->set_ang_vel_pitch_max_degs(max_rate);
break;
case YAW:
case YAW_D:
@ -598,6 +632,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i
attitude_control->get_rate_yaw_pid().filt_E_hz(rate_flte);
attitude_control->get_angle_yaw_p().kP(angle_p);
attitude_control->set_accel_yaw_max_cdss(max_accel);
attitude_control->set_ang_vel_yaw_max_degs(max_rate);
break;
}
}
@ -619,7 +654,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
// sanity check the rate P values
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) {
load_gain_set(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);
load_gain_set(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);
// save rate roll gains
attitude_control->get_rate_roll_pid().save_gains();
@ -636,7 +671,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) {
load_gain_set(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);
load_gain_set(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);
// save rate pitch gains
attitude_control->get_rate_pitch_pid().save_gains();
@ -653,7 +688,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
load_gain_set(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, orig_yaw_rLPF, orig_yaw_smax);
load_gain_set(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, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate);
// save rate yaw gains
attitude_control->get_rate_yaw_pid().save_gains();
@ -702,12 +737,14 @@ void AC_AutoTune_Heli::report_axis_gains(const char* axis_string, float rate_P,
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f",axis_string,angle_P,max_accel);
}
void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type)
void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type)
{
test_input_type = waveform_input_type;
test_freq_resp_input = freq_resp_input;
test_calc_type = calc_type;
test_start_freq = start_frq;
//target attitude magnitude
tgt_attitude = amplitude * 0.01745f;
// initialize frequency response object
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
@ -744,10 +781,10 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi
filt_target_rate = 0.0f;
// filter at lower frequency to remove steady state
filt_command_reading.set_cutoff_frequency(0.2f * start_frq);
filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq);
filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq);
filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq);
filt_command_reading.set_cutoff_frequency(0.2f * filt_freq);
filt_gyro_reading.set_cutoff_frequency(0.05f * filt_freq);
filt_tgt_rate_reading.set_cutoff_frequency(0.05f * filt_freq);
filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * filt_freq);
curr_test_mtr = {};
curr_test_tgt = {};
@ -762,7 +799,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
float gyro_reading = 0.0f;
float command_reading = 0.0f;
float tgt_rate_reading = 0.0f;
float tgt_attitude;
const uint32_t now = AP_HAL::millis();
float target_angle_cd = 0.0f;
float dwell_freq = test_start_freq;
@ -772,9 +808,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
cycle_time_ms = 1000.0f * M_2PI / dwell_freq;
}
//Determine target attitude magnitude limiting acceleration and rate
tgt_attitude = 5.0f * 0.01745f;
// body frame calculation of velocity
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
@ -1059,6 +1092,10 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
// if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
if (!is_zero(sweep_tgt.maxgain.freq)) {
next_test_freq = constrain_float(sweep_tgt.maxgain.freq, min_sweep_freq, max_sweep_freq);
freq_max = next_test_freq;
sp_prev_gain = sweep_tgt.maxgain.gain;
phase_max = sweep_tgt.maxgain.phase;
found_max_gain_freq = true;
} else {
next_test_freq = min_sweep_freq;
}
@ -1173,27 +1210,37 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
// FF is adjusted until rate requested is achieved
void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq)
{
float test_freq_incr = 0.05f * M_2PI;
float tune_tgt = 0.95;
float tune_tol = 0.025;
next_freq = test_data.freq;
if (test_data.phase > 15.0f) {
next_freq = constrain_float(next_freq - test_freq_incr, 0.1 * M_2PI, max_sweep_freq);
} else if (test_data.phase < 0.0f) {
next_freq = constrain_float(next_freq + test_freq_incr, 0.1 * M_2PI, max_sweep_freq);
} else {
if ((test_data.gain > 0.1 && test_data.gain < 0.93) || test_data.gain > 0.98) {
if (tune_ff > 0.0f) {
tune_ff = 0.95f * tune_ff / test_data.gain;
} else {
// handle axes where FF gain is initially zero
if (test_data.gain < tune_tgt - tune_tol && !is_positive(tune_ff)) {
tune_ff = 0.03f;
return;
}
} else if (test_data.gain >= 0.93 && test_data.gain <= 0.98) {
if (test_data.gain < tune_tgt - 0.2 || test_data.gain > tune_tgt + 0.2) {
tune_ff = tune_ff * constrain_float(tune_tgt / test_data.gain, 0.75, 1.25); //keep changes less than 25%
} else if (test_data.gain < tune_tgt - 0.1 || test_data.gain > tune_tgt + 0.1) {
if (test_data.gain < tune_tgt - 0.1) {
tune_ff *= 1.05;
} else {
tune_ff *= 0.95;
}
} else if (test_data.gain < tune_tgt - tune_tol || test_data.gain > tune_tgt + tune_tol) {
if (test_data.gain < tune_tgt - tune_tol) {
tune_ff *= 1.02;
} else {
tune_ff *= 0.98;
}
} else if (test_data.gain >= tune_tgt - tune_tol && test_data.gain <= tune_tgt + tune_tol) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset next_freq for next test
next_freq = 0.0f;
tune_ff = constrain_float(tune_ff,0.0f,1.0f);
}
}
}
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p)
@ -1276,7 +1323,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data,
next_freq = test_data.freq + test_freq_incr;
return;
// Gain is expected to continue decreasing past gain peak. declare max gain freq found and refine search.
} else if (test_data.gain < 0.9f * sp_prev_gain) {
} else if (test_data.gain < 0.95f * sp_prev_gain) {
found_max_gain_freq = true;
next_freq = freq_max + 0.5 * test_freq_incr;
return;

5
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -50,7 +50,7 @@ protected:
void backup_gains_and_initialise() override;
// load gains
void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax);
void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax, float max_rate);
// switch to use original gains
void load_orig_gains() override;
@ -176,7 +176,7 @@ private:
float angle_lim_neg_rpy_cd() const override;
// initialize dwell test or angle dwell test variables
void dwell_test_init(float start_frq, float stop_frq, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type);
void dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_run(sweep_info &test_data);
@ -248,6 +248,7 @@ private:
FreqRespInput test_freq_resp_input;
uint8_t num_dwell_cycles;
float test_start_freq;
float tgt_attitude;
float pre_calc_cycles; // number of cycles to complete before running frequency response calculations
float command_out; // test axis command output