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 LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values // 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_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; 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; 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; bool orig_bf_feedforward;
// currently being tuned parameter values // currently being tuned parameter values

141
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; AC_AutoTune_FreqResp::ResponseType resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
FreqRespCalcType calc_type = RATE; FreqRespCalcType calc_type = RATE;
FreqRespInput freq_resp_input = TARGET; FreqRespInput freq_resp_input = TARGET;
float freq_resp_amplitude = 5.0f; // amplitude in deg
float filter_freq = 10.0f;
switch (tune_type) { switch (tune_type) {
case RFF_UP: case RFF_UP:
if (!is_positive(next_test_freq)) { if (!is_positive(next_test_freq)) {
@ -150,10 +152,12 @@ void AC_AutoTune_Heli::test_init()
start_freq = next_test_freq; start_freq = next_test_freq;
} }
stop_freq = start_freq; stop_freq = start_freq;
filter_freq = start_freq;
attitude_control->bf_feedforward(false); attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(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 // variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
calc_type = RATE; calc_type = RATE;
@ -172,6 +176,7 @@ void AC_AutoTune_Heli::test_init()
stop_freq = start_freq; stop_freq = start_freq;
test_accel_max = 0.0f; test_accel_max = 0.0f;
} }
filter_freq = start_freq;
attitude_control->bf_feedforward(false); attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false); attitude_control->use_sqrt_controller(false);
@ -201,6 +206,7 @@ void AC_AutoTune_Heli::test_init()
start_freq = next_test_freq; start_freq = next_test_freq;
} }
stop_freq = start_freq; stop_freq = start_freq;
filter_freq = start_freq;
attitude_control->bf_feedforward(false); attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false); attitude_control->use_sqrt_controller(false);
@ -223,6 +229,7 @@ void AC_AutoTune_Heli::test_init()
stop_freq = start_freq; stop_freq = start_freq;
test_accel_max = 0.0f; test_accel_max = 0.0f;
} }
filter_freq = start_freq;
attitude_control->bf_feedforward(false); attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false); attitude_control->use_sqrt_controller(false);
@ -238,6 +245,7 @@ void AC_AutoTune_Heli::test_init()
start_freq = min_sweep_freq; start_freq = min_sweep_freq;
stop_freq = max_sweep_freq; stop_freq = max_sweep_freq;
test_accel_max = 0.0f; test_accel_max = 0.0f;
filter_freq = start_freq;
// variables needed to initialize frequency response object and test method // variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE; resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE;
@ -256,7 +264,7 @@ void AC_AutoTune_Heli::test_init()
// initialize dwell test method // 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 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_smax = attitude_control->get_rate_roll_pid().slew_limit();
orig_roll_sp = attitude_control->get_angle_roll_p().kP(); orig_roll_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max_cdss(); 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_rp = attitude_control->get_rate_roll_pid().kP();
tune_roll_rd = attitude_control->get_rate_roll_pid().kD(); tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
tune_roll_rff = attitude_control->get_rate_roll_pid().ff(); 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_smax = attitude_control->get_rate_pitch_pid().slew_limit();
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss(); 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_rp = attitude_control->get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
tune_pitch_rff = attitude_control->get_rate_pitch_pid().ff(); 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_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); 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_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
tune_yaw_rff = attitude_control->get_rate_yaw_pid().ff(); 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); attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) { 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()) { 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()) { 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); attitude_control->set_accel_pitch_max_cdss(0.0f);
} }
if (roll_enabled()) { 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()) { 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 (yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) { 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 // sanity check the gains
attitude_control->bf_feedforward(true); attitude_control->bf_feedforward(true);
if (roll_enabled()) { 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()) { 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()) { 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) // called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli::load_test_gains() 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) { switch (axis) {
case ROLL: case ROLL:
if (tune_type == SP_UP || tune_type == TUNE_CHECK) { 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_p = tune_roll_rp;
rate_d = tune_roll_rd; 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; break;
case PITCH: case PITCH:
if (tune_type == SP_UP || tune_type == TUNE_CHECK) { 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_p = tune_pitch_rp;
rate_d = tune_pitch_rd; 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; break;
case YAW: case YAW:
case YAW_D: 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 // freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_yaw_ri; 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; break;
} }
} }
// load gains // 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) { switch (s_axis) {
case ROLL: 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_rate_roll_pid().slew_limit(smax);
attitude_control->get_angle_roll_p().kP(angle_p); attitude_control->get_angle_roll_p().kP(angle_p);
attitude_control->set_accel_roll_max_cdss(max_accel); attitude_control->set_accel_roll_max_cdss(max_accel);
attitude_control->set_ang_vel_roll_max_degs(max_rate);
break; break;
case PITCH: case PITCH:
attitude_control->get_rate_pitch_pid().kP(rate_p); 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_rate_pitch_pid().slew_limit(smax);
attitude_control->get_angle_pitch_p().kP(angle_p); attitude_control->get_angle_pitch_p().kP(angle_p);
attitude_control->set_accel_pitch_max_cdss(max_accel); attitude_control->set_accel_pitch_max_cdss(max_accel);
attitude_control->set_ang_vel_pitch_max_degs(max_rate);
break; break;
case YAW: case YAW:
case YAW_D: 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_rate_yaw_pid().filt_E_hz(rate_flte);
attitude_control->get_angle_yaw_p().kP(angle_p); attitude_control->get_angle_yaw_p().kP(angle_p);
attitude_control->set_accel_yaw_max_cdss(max_accel); attitude_control->set_accel_yaw_max_cdss(max_accel);
attitude_control->set_ang_vel_yaw_max_degs(max_rate);
break; break;
} }
} }
@ -619,7 +654,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
// sanity check the rate P values // sanity check the rate P values
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { 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 // save rate roll gains
attitude_control->get_rate_roll_pid().save_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()) { 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 // save rate pitch gains
attitude_control->get_rate_pitch_pid().save_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)) { 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 // save rate yaw gains
attitude_control->get_rate_yaw_pid().save_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); 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_input_type = waveform_input_type;
test_freq_resp_input = freq_resp_input; test_freq_resp_input = freq_resp_input;
test_calc_type = calc_type; test_calc_type = calc_type;
test_start_freq = start_frq; test_start_freq = start_frq;
//target attitude magnitude
tgt_attitude = amplitude * 0.01745f;
// initialize frequency response object // initialize frequency response object
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { 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; filt_target_rate = 0.0f;
// filter at lower frequency to remove steady state // filter at lower frequency to remove steady state
filt_command_reading.set_cutoff_frequency(0.2f * start_frq); filt_command_reading.set_cutoff_frequency(0.2f * filt_freq);
filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq); filt_gyro_reading.set_cutoff_frequency(0.05f * filt_freq);
filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq); filt_tgt_rate_reading.set_cutoff_frequency(0.05f * filt_freq);
filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq); filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * filt_freq);
curr_test_mtr = {}; curr_test_mtr = {};
curr_test_tgt = {}; curr_test_tgt = {};
@ -762,7 +799,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
float gyro_reading = 0.0f; float gyro_reading = 0.0f;
float command_reading = 0.0f; float command_reading = 0.0f;
float tgt_rate_reading = 0.0f; float tgt_rate_reading = 0.0f;
float tgt_attitude;
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
float target_angle_cd = 0.0f; float target_angle_cd = 0.0f;
float dwell_freq = test_start_freq; 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; 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 // body frame calculation of velocity
Vector3f velocity_ned, velocity_bf; Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) { 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 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)) { if (!is_zero(sweep_tgt.maxgain.freq)) {
next_test_freq = constrain_float(sweep_tgt.maxgain.freq, min_sweep_freq, max_sweep_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 { } else {
next_test_freq = min_sweep_freq; next_test_freq = min_sweep_freq;
} }
@ -1173,25 +1210,35 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
// FF is adjusted until rate requested is achieved // 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) 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; 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); // handle axes where FF gain is initially zero
} else if (test_data.phase < 0.0f) { if (test_data.gain < tune_tgt - tune_tol && !is_positive(tune_ff)) {
next_freq = constrain_float(next_freq + test_freq_incr, 0.1 * M_2PI, max_sweep_freq); tune_ff = 0.03f;
} else { return;
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; if (test_data.gain < tune_tgt - 0.2 || test_data.gain > tune_tgt + 0.2) {
} else { tune_ff = tune_ff * constrain_float(tune_tgt / test_data.gain, 0.75, 1.25); //keep changes less than 25%
tune_ff = 0.03f; } else if (test_data.gain < tune_tgt - 0.1 || test_data.gain > tune_tgt + 0.1) {
} if (test_data.gain < tune_tgt - 0.1) {
} else if (test_data.gain >= 0.93 && test_data.gain <= 0.98) { tune_ff *= 1.05;
counter = AUTOTUNE_SUCCESS_COUNT; } else {
// reset next_freq for next test tune_ff *= 0.95;
next_freq = 0.0f;
tune_ff = constrain_float(tune_ff,0.0f,1.0f);
} }
} 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);
} }
} }
@ -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; next_freq = test_data.freq + test_freq_incr;
return; return;
// Gain is expected to continue decreasing past gain peak. declare max gain freq found and refine search. // 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; found_max_gain_freq = true;
next_freq = freq_max + 0.5 * test_freq_incr; next_freq = freq_max + 0.5 * test_freq_incr;
return; return;

5
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -50,7 +50,7 @@ protected:
void backup_gains_and_initialise() override; void backup_gains_and_initialise() override;
// load gains // 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 // switch to use original gains
void load_orig_gains() override; void load_orig_gains() override;
@ -176,7 +176,7 @@ private:
float angle_lim_neg_rpy_cd() const override; float angle_lim_neg_rpy_cd() const override;
// initialize dwell test or angle dwell test variables // 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 // dwell test used to perform frequency dwells for rate gains
void dwell_test_run(sweep_info &test_data); void dwell_test_run(sweep_info &test_data);
@ -248,6 +248,7 @@ private:
FreqRespInput test_freq_resp_input; FreqRespInput test_freq_resp_input;
uint8_t num_dwell_cycles; uint8_t num_dwell_cycles;
float test_start_freq; float test_start_freq;
float tgt_attitude;
float pre_calc_cycles; // number of cycles to complete before running frequency response calculations float pre_calc_cycles; // number of cycles to complete before running frequency response calculations
float command_out; // test axis command output float command_out; // test axis command output