mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AC_AutoTune: consolidate gcs messages and add reset for update gain variables
This commit is contained in:
Bill Geyer
Bill Geyer
parent
9945c80fb4
commit
fb5fec387e
@ -429,6 +429,10 @@ void AC_AutoTune::control_attitude()
|
||||
// log the latest gains
|
||||
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) {
|
||||
// Check results after mini-step to increase rate D gain
|
||||
case RD_UP:
|
||||
@ -516,6 +520,7 @@ void AC_AutoTune::control_attitude()
|
||||
AP_Notify::events.autotune_complete = true;
|
||||
} else {
|
||||
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
|
||||
axes_completed = 0;
|
||||
|
||||
// reset update gain variables for each vehicle
|
||||
reset_update_gain_variables();
|
||||
|
||||
// start at the beginning of tune sequence
|
||||
next_tune_type(tune_type, true);
|
||||
|
||||
|
||||
@ -112,6 +112,9 @@ protected:
|
||||
// reset the test vaariables for each vehicle
|
||||
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
|
||||
virtual void test_init() = 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
|
||||
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)
|
||||
void update_gcs(uint8_t message_id) const;
|
||||
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
#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_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_SP_MAX 10.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) {
|
||||
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 = '?';
|
||||
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;
|
||||
axis_char = 'R';
|
||||
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;
|
||||
axis_char = 'P';
|
||||
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;
|
||||
axis_char = 'Y';
|
||||
break;
|
||||
}
|
||||
@ -251,33 +231,104 @@ void AC_AutoTune_Heli::do_gcs_announcements()
|
||||
send_step_string();
|
||||
switch (tune_type) {
|
||||
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:
|
||||
// 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 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;
|
||||
}
|
||||
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
|
||||
|
||||
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
|
||||
// called before tuning starts to backup original gains
|
||||
void AC_AutoTune_Heli::backup_gains_and_initialise()
|
||||
@ -288,7 +339,6 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
|
||||
freq_cnt = 0;
|
||||
start_freq = 0.0f;
|
||||
stop_freq = 0.0f;
|
||||
ff_up_first_iter = true;
|
||||
|
||||
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();
|
||||
// log sweep data
|
||||
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 {
|
||||
dwell_gain = freqresp_rate.get_gain();
|
||||
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) {
|
||||
// we have passed the maximum stop time
|
||||
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 {
|
||||
@ -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_freq = 0.0f;
|
||||
sweep.maxgain_phase = 0.0f;
|
||||
sweep.progress = 0;
|
||||
curr_test_gain = 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();
|
||||
// log sweep data
|
||||
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 {
|
||||
dwell_gain = freqresp_angle.get_gain();
|
||||
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
|
||||
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) {
|
||||
case ROLL:
|
||||
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
|
||||
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) {
|
||||
case ROLL:
|
||||
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
|
||||
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) {
|
||||
case ROLL:
|
||||
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
|
||||
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) {
|
||||
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);
|
||||
@ -1450,37 +1457,31 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
|
||||
{
|
||||
switch (tune_type) {
|
||||
case RD_UP:
|
||||
break;
|
||||
case RD_DOWN:
|
||||
switch (test_axis) {
|
||||
case ROLL:
|
||||
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;
|
||||
case PITCH:
|
||||
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;
|
||||
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;
|
||||
case RP_UP:
|
||||
switch (test_axis) {
|
||||
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;
|
||||
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;
|
||||
case YAW:
|
||||
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case SP_DOWN:
|
||||
break;
|
||||
case SP_UP:
|
||||
switch (test_axis) {
|
||||
case ROLL:
|
||||
@ -1495,8 +1496,8 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
|
||||
}
|
||||
break;
|
||||
case RFF_UP:
|
||||
case MAX_GAINS:
|
||||
case TUNE_COMPLETE:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -1522,8 +1523,6 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
|
||||
counter = AUTOTUNE_SUCCESS_COUNT;
|
||||
tune_ff = 0.95f * 0.5 * (tune_ff + first_dir_rff);
|
||||
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)) {
|
||||
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;
|
||||
counter = AUTOTUNE_SUCCESS_COUNT;
|
||||
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)) {
|
||||
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;
|
||||
counter = AUTOTUNE_SUCCESS_COUNT;
|
||||
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
|
||||
ff_up_first_iter = true;
|
||||
first_dir_complete = false;
|
||||
}
|
||||
} else {
|
||||
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];
|
||||
}
|
||||
} 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)) {
|
||||
tune_p = 0.05f * max_gain_p.max_allowed;
|
||||
} 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;
|
||||
tune_p -= 0.05f * 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) {
|
||||
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)) {
|
||||
tune_p = 0.05f * max_gain_p.max_allowed;
|
||||
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];
|
||||
}
|
||||
} 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)) {
|
||||
tune_d = 0.05f * max_gain_d.max_allowed;
|
||||
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) {
|
||||
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)) {
|
||||
tune_d = 0.05f * max_gain_d.max_allowed;
|
||||
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
|
||||
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 &&
|
||||
!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);
|
||||
found_max_d = true;
|
||||
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.
|
||||
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
|
||||
curr_test_freq = freq[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
|
||||
} else {
|
||||
if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) {
|
||||
@ -2002,11 +1982,19 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
|
||||
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
|
||||
void AC_AutoTune_Heli::Log_AutoTune()
|
||||
{
|
||||
|
||||
switch (axis) {
|
||||
case ROLL:
|
||||
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);
|
||||
@ -2018,7 +2006,6 @@ 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_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
|
||||
@ -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
|
||||
void AC_AutoTune_Heli::set_tune_sequence()
|
||||
{
|
||||
|
||||
@ -56,6 +56,9 @@ protected:
|
||||
// reset the test vaariables for heli
|
||||
void reset_vehicle_test_variables() override;
|
||||
|
||||
// reset the update gain variables for heli
|
||||
void reset_update_gain_variables() override;
|
||||
|
||||
// initializes test
|
||||
void test_init() override;
|
||||
|
||||
@ -104,6 +107,9 @@ protected:
|
||||
// send intermittant updates to user on status of tune
|
||||
void do_gcs_announcements() override;
|
||||
|
||||
// send post test updates to user
|
||||
void do_post_test_gcs_announcements() override;
|
||||
|
||||
// set the tuning test sequence
|
||||
void set_tune_sequence() override;
|
||||
|
||||
|
||||
@ -53,16 +53,21 @@ protected:
|
||||
// load test gains
|
||||
void load_test_gains() override;
|
||||
|
||||
// reset the test vaariables for heli
|
||||
void reset_vehicle_test_variables() override {
|
||||
// this should never happen
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
// reset the test vaariables for multi
|
||||
void reset_vehicle_test_variables() override {};
|
||||
|
||||
// reset the update gain variables for multi
|
||||
void reset_update_gain_variables() override {};
|
||||
|
||||
void test_init() 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;
|
||||
|
||||
// send post test updates to user
|
||||
void do_post_test_gcs_announcements() override {};
|
||||
|
||||
// update gains for the rate P up tune type
|
||||
void updating_rate_p_up_all(AxisType test_axis) override;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user