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: consolidate gcs messages and add reset for update gain variables

Browse Source
This commit is contained in:
Bill Geyer 2022-01-07 13:37:02 -05:00 committed by Bill Geyer
parent 9945c80fb4
commit fb5fec387e
5 changed files with 168 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -429,6 +429,10 @@ void AC_AutoTune::control_attitude()
// log the latest gains // log the latest gains
Log_AutoTune(); Log_AutoTune();
// Announce tune type test results
// must be done before updating method because this method changes parameters for next test
do_post_test_gcs_announcements();
switch (tune_type) { switch (tune_type) {
// Check results after mini-step to increase rate D gain // Check results after mini-step to increase rate D gain
case RD_UP: case RD_UP:
@ -516,6 +520,7 @@ void AC_AutoTune::control_attitude()
AP_Notify::events.autotune_complete = true; AP_Notify::events.autotune_complete = true;
} else { } else {
AP_Notify::events.autotune_next_axis = true; AP_Notify::events.autotune_next_axis = true;
reset_update_gain_variables();
} }
} }
} }
@ -554,6 +559,9 @@ void AC_AutoTune::backup_gains_and_initialise()
// no axes are complete // no axes are complete
axes_completed = 0; axes_completed = 0;
// reset update gain variables for each vehicle
reset_update_gain_variables();
// start at the beginning of tune sequence // start at the beginning of tune sequence
next_tune_type(tune_type, true); next_tune_type(tune_type, true);

6
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -112,6 +112,9 @@ protected:
// reset the test vaariables for each vehicle // reset the test vaariables for each vehicle
virtual void reset_vehicle_test_variables() = 0; virtual void reset_vehicle_test_variables() = 0;
// reset the update gain variables for each vehicle
virtual void reset_update_gain_variables() = 0;
// test initialization and run methods that should be overridden for each vehicle // test initialization and run methods that should be overridden for each vehicle
virtual void test_init() = 0; virtual void test_init() = 0;
virtual void test_run(AxisType test_axis, const float dir_sign) = 0; virtual void test_run(AxisType test_axis, const float dir_sign) = 0;
@ -156,6 +159,9 @@ protected:
// send intermittant updates to user on status of tune // send intermittant updates to user on status of tune
virtual void do_gcs_announcements() = 0; virtual void do_gcs_announcements() = 0;
// send post test updates to user
virtual void do_post_test_gcs_announcements() = 0;
// send message with high level status (e.g. Started, Stopped) // send message with high level status (e.g. Started, Stopped)
void update_gcs(uint8_t message_id) const; void update_gcs(uint8_t message_id) const;

267
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -28,7 +28,7 @@
#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing #define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing #define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
#define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value #define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value
#define AUTOTUNE_RP_MIN 0.001f // minimum Rate P value #define AUTOTUNE_RP_MIN 0.02f // minimum Rate P value
#define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value #define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value
#define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value #define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value
#define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value #define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value
@ -214,35 +214,15 @@ void AC_AutoTune_Heli::do_gcs_announcements()
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
return; return;
} }
float tune_rp = 0.0f;
float tune_rd = 0.0f;
float tune_rff = 0.0f;
float tune_sp = 0.0f;
float tune_accel = 0.0f;
char axis_char = '?'; char axis_char = '?';
switch (axis) { switch (axis) {
case ROLL: case ROLL:
tune_rp = tune_roll_rp;
tune_rd = tune_roll_rd;
tune_rff = tune_roll_rff;
tune_sp = tune_roll_sp;
tune_accel = tune_roll_accel;
axis_char = 'R'; axis_char = 'R';
break; break;
case PITCH: case PITCH:
tune_rp = tune_pitch_rp;
tune_rd = tune_pitch_rd;
tune_rff = tune_pitch_rff;
tune_sp = tune_pitch_sp;
tune_accel = tune_pitch_accel;
axis_char = 'P'; axis_char = 'P';
break; break;
case YAW: case YAW:
tune_rp = tune_yaw_rp;
tune_rd = tune_yaw_rd;
tune_rff = tune_yaw_rff;
tune_sp = tune_yaw_sp;
tune_accel = tune_yaw_accel;
axis_char = 'Y'; axis_char = 'Y';
break; break;
} }
@ -251,33 +231,104 @@ void AC_AutoTune_Heli::do_gcs_announcements()
send_step_string(); send_step_string();
switch (tune_type) { switch (tune_type) {
case RD_UP: case RD_UP:
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f d=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)tune_rd);
break;
case RD_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case RP_UP: case RP_UP:
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f p=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)tune_rp);
break;
case RFF_UP:
if (!is_zero(test_rate_filt)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: target=%f rotation=%f command=%f", (double)(test_tgt_rate_filt*57.3f), (double)(test_rate_filt*57.3f), (double)(test_command_filt));
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff);
break;
case SP_DOWN:
case SP_UP:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel);
break;
case MAX_GAINS: case MAX_GAINS:
case TUNE_COMPLETE: case SP_UP:
if (is_equal(start_freq,stop_freq)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Dwell");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep");
if (settle_time == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
}
}
break;
default:
break; break;
} }
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
announce_time = now; announce_time = now;
} }
// send post test updates to user
void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
float tune_rp = 0.0f;
float tune_rd = 0.0f;
float tune_rff = 0.0f;
float tune_sp = 0.0f;
float tune_accel = 0.0f;
switch (axis) {
case ROLL:
tune_rp = tune_roll_rp;
tune_rd = tune_roll_rd;
tune_rff = tune_roll_rff;
tune_sp = tune_roll_sp;
tune_accel = tune_roll_accel;
break;
case PITCH:
tune_rp = tune_pitch_rp;
tune_rd = tune_pitch_rd;
tune_rff = tune_pitch_rff;
tune_sp = tune_pitch_sp;
tune_accel = tune_pitch_accel;
break;
case YAW:
tune_rp = tune_yaw_rp;
tune_rd = tune_yaw_rd;
tune_rff = tune_yaw_rff;
tune_sp = tune_yaw_sp;
tune_accel = tune_yaw_accel;
break;
}
if (step == UPDATE_GAINS) {
switch (tune_type) {
case RFF_UP:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: target=%f rotation=%f command=%f", (double)(test_tgt_rate_filt*57.3f), (double)(test_rate_filt*57.3f), (double)(test_command_filt));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff);
break;
case RP_UP:
if (is_equal(start_freq,stop_freq)) {
// announce results of dwell
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_rp));
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
}
break;
case RD_UP:
// announce results of dwell
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(test_phase[freq_cnt]), (double)(tune_rd));
break;
case SP_UP:
if (is_equal(start_freq,stop_freq)) {
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f angle_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_sp));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tune_accel=%f max_accel=%f", (double)(tune_accel), (double)(test_accel_max));
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
}
break;
case MAX_GAINS:
if (is_equal(start_freq,stop_freq)) {
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]));
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
}
break;
default:
break;
}
}
}
// backup_gains_and_initialise - store current gains as originals // backup_gains_and_initialise - store current gains as originals
// called before tuning starts to backup original gains // called before tuning starts to backup original gains
void AC_AutoTune_Heli::backup_gains_and_initialise() void AC_AutoTune_Heli::backup_gains_and_initialise()
@ -288,7 +339,6 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
freq_cnt = 0; freq_cnt = 0;
start_freq = 0.0f; start_freq = 0.0f;
stop_freq = 0.0f; stop_freq = 0.0f;
ff_up_first_iter = true;
orig_bf_feedforward = attitude_control->get_bf_feedforward(); orig_bf_feedforward = attitude_control->get_bf_feedforward();
@ -1026,7 +1076,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
freqresp_rate.reset_cycle_complete(); freqresp_rate.reset_cycle_complete();
// log sweep data // log sweep data
Log_AutoTuneSweep(); Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else { } else {
dwell_gain = freqresp_rate.get_gain(); dwell_gain = freqresp_rate.get_gain();
dwell_phase = freqresp_rate.get_phase(); dwell_phase = freqresp_rate.get_phase();
@ -1061,8 +1110,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
if (now - step_start_time_ms >= sweep_time_ms + 200) { if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time // we have passed the maximum stop time
step = UPDATE_GAINS; step = UPDATE_GAINS;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
} }
} else { } else {
@ -1125,6 +1172,7 @@ void AC_AutoTune_Heli::angle_dwell_test_init(float start_frq, float filt_freq)
sweep.maxgain_gain = 0.0f; sweep.maxgain_gain = 0.0f;
sweep.maxgain_freq = 0.0f; sweep.maxgain_freq = 0.0f;
sweep.maxgain_phase = 0.0f; sweep.maxgain_phase = 0.0f;
sweep.progress = 0;
curr_test_gain = 0.0f; curr_test_gain = 0.0f;
curr_test_phase = 0.0f; curr_test_phase = 0.0f;
} }
@ -1236,7 +1284,6 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
freqresp_angle.reset_cycle_complete(); freqresp_angle.reset_cycle_complete();
// log sweep data // log sweep data
Log_AutoTuneSweep(); Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else { } else {
dwell_gain = freqresp_angle.get_gain(); dwell_gain = freqresp_angle.get_gain();
dwell_phase = freqresp_angle.get_phase(); dwell_phase = freqresp_angle.get_phase();
@ -1317,22 +1364,6 @@ float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_m
// update gains for the rate p up tune type // update gains for the rate p up tune type
void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis) void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
{ {
float p_gain = 0.0f;
switch (test_axis) {
case ROLL:
p_gain = tune_roll_rp;
break;
case PITCH:
p_gain = tune_pitch_rp;
break;
case YAW:
p_gain = tune_yaw_rp;
break;
}
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f rate_p=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)(p_gain));
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
updating_rate_p_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); updating_rate_p_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
@ -1349,22 +1380,6 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
// update gains for the rate d up tune type // update gains for the rate d up tune type
void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis) void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
{ {
float d_gain = 0.0f;
switch (test_axis) {
case ROLL:
d_gain = tune_roll_rd;
break;
case PITCH:
d_gain = tune_pitch_rd;
break;
case YAW:
d_gain = tune_yaw_rd;
break;
}
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f rate_d=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)(d_gain));
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
updating_rate_d_up(tune_roll_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d); updating_rate_d_up(tune_roll_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
@ -1401,10 +1416,6 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
// update gains for the angle p up tune type // update gains for the angle p up tune type
void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis) void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
{ {
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f accel=%f", (double)(test_phase[freq_cnt]), (double)(test_accel_max));
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
updating_angle_p_up(tune_roll_sp, test_freq, test_gain, test_phase, freq_cnt); updating_angle_p_up(tune_roll_sp, test_freq, test_gain, test_phase, freq_cnt);
@ -1421,10 +1432,6 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
// update gains for the max gain tune type // update gains for the max gain tune type
void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
{ {
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]));
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd); updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd);
@ -1450,37 +1457,31 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
{ {
switch (tune_type) { switch (tune_type) {
case RD_UP: case RD_UP:
break;
case RD_DOWN:
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF); tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break; break;
case PITCH: case PITCH:
tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break; break;
case YAW: case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); tune_yaw_rd = MAX(0.0f, tune_yaw_rd * AUTOTUNE_RD_BACKOFF);
break; break;
} }
break; break;
case RP_UP: case RP_UP:
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); tune_roll_rp = MAX(0.0f, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
break; break;
case PITCH: case PITCH:
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); tune_pitch_rp = MAX(0.0f, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break; break;
case YAW: case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break; break;
} }
break; break;
case SP_DOWN:
break;
case SP_UP: case SP_UP:
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
@ -1495,8 +1496,8 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
} }
break; break;
case RFF_UP: case RFF_UP:
case MAX_GAINS: break;
case TUNE_COMPLETE: default:
break; break;
} }
} }
@ -1522,8 +1523,6 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
tune_ff = 0.95f * 0.5 * (tune_ff + first_dir_rff); tune_ff = 0.95f * 0.5 * (tune_ff + first_dir_rff);
tune_ff = constrain_float(tune_ff, AUTOTUNE_RFF_MIN, AUTOTUNE_RFF_MAX); tune_ff = constrain_float(tune_ff, AUTOTUNE_RFF_MIN, AUTOTUNE_RFF_MAX);
ff_up_first_iter = true;
first_dir_complete = false;
} }
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) > 1.05f * fabsf(rate_target)) { } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) > 1.05f * fabsf(rate_target)) {
tune_ff = 0.98f * tune_ff; tune_ff = 0.98f * tune_ff;
@ -1531,8 +1530,6 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
tune_ff = AUTOTUNE_RFF_MIN; tune_ff = AUTOTUNE_RFF_MIN;
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
ff_up_first_iter = true;
first_dir_complete = false;
} }
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) { } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) {
tune_ff = 1.02f * tune_ff; tune_ff = 1.02f * tune_ff;
@ -1540,8 +1537,6 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
tune_ff = AUTOTUNE_RFF_MAX; tune_ff = AUTOTUNE_RFF_MAX;
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
ff_up_first_iter = true;
first_dir_complete = false;
} }
} else { } else {
if (!is_zero(meas_rate)) { if (!is_zero(meas_rate)) {
@ -1578,7 +1573,6 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai
curr_test_freq = freq[frq_cnt]; curr_test_freq = freq[frq_cnt];
} }
} else if (is_equal(start_freq,stop_freq) && method == 2) { } else if (is_equal(start_freq,stop_freq) && method == 2) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt]));
if (is_zero(tune_p)) { if (is_zero(tune_p)) {
tune_p = 0.05f * max_gain_p.max_allowed; tune_p = 0.05f * max_gain_p.max_allowed;
} else if (phase[frq_cnt] > 180.0f) { } else if (phase[frq_cnt] > 180.0f) {
@ -1597,13 +1591,9 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai
frq_cnt = 0; frq_cnt = 0;
tune_p -= 0.05f * max_gain_p.max_allowed; tune_p -= 0.05f * max_gain_p.max_allowed;
tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed);
// rp_prev_gain = 0.0f;
} }
} }
// rp_prev_gain = gain[frq_cnt];
} else if (is_equal(start_freq,stop_freq) && method == 1) { } else if (is_equal(start_freq,stop_freq) && method == 1) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt]));
if (is_zero(tune_p)) { if (is_zero(tune_p)) {
tune_p = 0.05f * max_gain_p.max_allowed; tune_p = 0.05f * max_gain_p.max_allowed;
rp_prev_gain = gain[frq_cnt]; rp_prev_gain = gain[frq_cnt];
@ -1677,7 +1667,6 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
curr_test_freq = freq[frq_cnt]; curr_test_freq = freq[frq_cnt];
} }
} else if (is_equal(start_freq,stop_freq) && method == 2) { } else if (is_equal(start_freq,stop_freq) && method == 2) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt]));
if (is_zero(tune_d)) { if (is_zero(tune_d)) {
tune_d = 0.05f * max_gain_d.max_allowed; tune_d = 0.05f * max_gain_d.max_allowed;
rd_prev_gain = gain[frq_cnt]; rd_prev_gain = gain[frq_cnt];
@ -1702,7 +1691,6 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
} }
} }
} else if (is_equal(start_freq,stop_freq) && method == 1) { } else if (is_equal(start_freq,stop_freq) && method == 1) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt]));
if (is_zero(tune_d)) { if (is_zero(tune_d)) {
tune_d = 0.05f * max_gain_d.max_allowed; tune_d = 0.05f * max_gain_d.max_allowed;
rd_prev_gain = gain[frq_cnt]; rd_prev_gain = gain[frq_cnt];
@ -1941,8 +1929,6 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
// set frequency back at least one test_freq_incr as it will be added // set frequency back at least one test_freq_incr as it will be added
freq[1] = sweep.ph270_freq - 1.5f * test_freq_incr; freq[1] = sweep.ph270_freq - 1.5f * test_freq_incr;
} }
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f ph=%f rate_p=%f", (double)(max_gain_p.freq), (double)(max_gain_p.gain), (double)(max_gain_p.phase), (double)(max_gain_p.max_allowed));
} }
if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f && if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f &&
!find_middle && !found_max_d && found_max_p) { !find_middle && !found_max_d && found_max_p) {
@ -1963,7 +1949,6 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
max_gain_d.max_allowed = constrain_float(max_gain_d.max_allowed, 0.0f, 2.0f * AUTOTUNE_RD_MAX); max_gain_d.max_allowed = constrain_float(max_gain_d.max_allowed, 0.0f, 2.0f * AUTOTUNE_RD_MAX);
found_max_d = true; found_max_d = true;
find_middle = false; find_middle = false;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f ph=%f rate_d=%f", (double)(max_gain_d.freq), (double)(max_gain_d.gain), (double)(max_gain_d.phase), (double)(max_gain_d.max_allowed));
} }
// stop progression in frequency. // stop progression in frequency.
if ((frq_cnt > 1 && phase[frq_cnt] > 330.0f && !is_zero(phase[frq_cnt])) || found_max_d) { if ((frq_cnt > 1 && phase[frq_cnt] > 330.0f && !is_zero(phase[frq_cnt])) || found_max_d) {
@ -1975,11 +1960,6 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
// reset variables for next test // reset variables for next test
curr_test_freq = freq[0]; curr_test_freq = freq[0];
frq_cnt = 0; frq_cnt = 0;
found_max_p = false;
found_max_d = false;
find_middle = false;
// tune_p = 0.35f * max_gain_p.max_allowed;
// tune_d = 0.25f * max_gain_d.max_allowed;
start_freq = 0.0f; //initializes next test that uses dwell test start_freq = 0.0f; //initializes next test that uses dwell test
} else { } else {
if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) { if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) {
@ -2002,23 +1982,30 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
stop_freq = curr_test_freq; stop_freq = curr_test_freq;
} }
} }
if (found_max_p && found_max_d) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f", (double)(max_rate_d.freq), (double)(max_rate_d.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(max_rate_d.phase), (double)(max_rate_d.max_allowed));
}
} }
// log autotune summary data // log autotune summary data
void AC_AutoTune_Heli::Log_AutoTune() void AC_AutoTune_Heli::Log_AutoTune()
{ {
switch (axis) {
case ROLL: switch (axis) {
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); case ROLL:
break; Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
case PITCH: break;
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); case PITCH:
break; 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);
case YAW: break;
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); case YAW:
break; 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;
// } }
} }
// log autotune time history results for command, angular rate, and attitude // log autotune time history results for command, angular rate, and attitude
@ -2133,6 +2120,28 @@ void AC_AutoTune_Heli::reset_vehicle_test_variables()
} }
} }
// reset the update gain variables for heli
void AC_AutoTune_Heli::reset_update_gain_variables()
{
// reset feedforward update gain variables
ff_up_first_iter = true;
first_dir_complete = false;
// reset max gain variables
max_rate_p.freq = 0.0f;
max_rate_p.gain = 0.0f;
max_rate_p.phase = 0.0f;
max_rate_p.max_allowed = 0.0f;
max_rate_d.freq = 0.0f;
max_rate_d.gain = 0.0f;
max_rate_d.phase = 0.0f;
max_rate_d.max_allowed = 0.0f;
found_max_p = false;
found_max_d = false;
find_middle = false;
}
// set the tuning test sequence // set the tuning test sequence
void AC_AutoTune_Heli::set_tune_sequence() void AC_AutoTune_Heli::set_tune_sequence()
{ {

6
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -56,6 +56,9 @@ protected:
// reset the test vaariables for heli // reset the test vaariables for heli
void reset_vehicle_test_variables() override; void reset_vehicle_test_variables() override;
// reset the update gain variables for heli
void reset_update_gain_variables() override;
// initializes test // initializes test
void test_init() override; void test_init() override;
@ -104,6 +107,9 @@ protected:
// send intermittant updates to user on status of tune // send intermittant updates to user on status of tune
void do_gcs_announcements() override; void do_gcs_announcements() override;
// send post test updates to user
void do_post_test_gcs_announcements() override;
// set the tuning test sequence // set the tuning test sequence
void set_tune_sequence() override; void set_tune_sequence() override;

15
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -53,16 +53,21 @@ protected:
// load test gains // load test gains
void load_test_gains() override; void load_test_gains() override;
// reset the test vaariables for heli // reset the test vaariables for multi
void reset_vehicle_test_variables() override { void reset_vehicle_test_variables() override {};
// this should never happen
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); // reset the update gain variables for multi
} void reset_update_gain_variables() override {};
void test_init() override; void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override; void test_run(AxisType test_axis, const float dir_sign) override;
// send intermittant updates to user on status of tune
void do_gcs_announcements() override; void do_gcs_announcements() override;
// send post test updates to user
void do_post_test_gcs_announcements() override {};
// update gains for the rate P up tune type // update gains for the rate P up tune type
void updating_rate_p_up_all(AxisType test_axis) override; void updating_rate_p_up_all(AxisType test_axis) override;