Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: make initialize methods to restart testing

This commit is contained in:
Bill Geyer 2021-12-22 22:37:31 -05:00 committed by Bill Geyer
parent 40321754fa
commit a9d47532d0
6 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -104,12 +104,6 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
FALLTHROUGH;
case UNINITIALISED:
// initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli
freq_cnt = 0;
start_freq = 0.0f;
stop_freq = 0.0f;
ff_up_first_iter = true;
// autotune has never been run
// so store current gains as original gains
backup_gains_and_initialise();
@ -120,14 +114,10 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
break;
case TUNING:
// reset test variables for each vehicle
reset_vehicle_test_variables();
// we are restarting tuning so restart where we left off
// reset test variables to continue where we left off
// reset dwell test variables if sweep was interrupted in order to restart sweep
if (!is_equal(start_freq, stop_freq)) {
freq_cnt = 0;
start_freq = 0.0f;
stop_freq = 0.0f;
}
step = WAITING_FOR_LEVEL;
step_start_time_ms = AP_HAL::millis();
level_start_time_ms = step_start_time_ms;
@ -228,12 +218,8 @@ const char *AC_AutoTune::type_string() const
return "Rate D Down";
case RP_UP:
return "Rate P Up";
case RP_DOWN:
return "Rate P Down";
case RFF_UP:
return "Rate FF Up";
case RFF_DOWN:
return "Rate FF Down";
case SP_UP:
return "Angle P Up";
case SP_DOWN:
@ -526,8 +512,6 @@ void AC_AutoTune::control_attitude()
case MAX_GAINS:
updating_max_gains_all(axis);
break;
case RP_DOWN:
case RFF_DOWN:
case TUNE_COMPLETE:
break;
}
@ -602,9 +586,7 @@ void AC_AutoTune::control_attitude()
break;
}
break;
case RP_DOWN:
case RFF_UP:
case RFF_DOWN:
case MAX_GAINS:
case TUNE_COMPLETE:
break;

15
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -115,6 +115,9 @@ protected:
// load gains for next test. relies on axis variable being set
virtual void load_test_gains() = 0;
// reset the test vaariables for each vehicle
virtual void reset_vehicle_test_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;
@ -201,13 +204,11 @@ protected:
RD_UP = 0, // rate D is being tuned up
RD_DOWN = 1, // rate D is being tuned down
RP_UP = 2, // rate P is being tuned up
RP_DOWN = 3, // rate P is being tuned down
RFF_UP = 4, // rate FF is being tuned up
RFF_DOWN = 5, // rate FF is being tuned down
SP_UP = 6, // angle P is being tuned up
SP_DOWN = 7, // angle P is being tuned down
MAX_GAINS = 8, // max allowable stable gains are determined
TUNE_COMPLETE = 9 // Reached end of tuning
RFF_UP = 3, // rate FF is being tuned up
SP_UP = 4, // angle P is being tuned up
SP_DOWN = 5, // angle P is being tuned down
MAX_GAINS = 6, // max allowable stable gains are determined
TUNE_COMPLETE = 7 // Reached end of tuning
};
TuneType tune_seq[6]; // holds sequence of tune_types to be performed
uint8_t tune_seq_curr; // current tune sequence step

25
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -102,7 +102,7 @@ AC_AutoTune_Heli::AC_AutoTune_Heli()
// initialize tests for each tune type
void AC_AutoTune_Heli::test_init()
{
if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) {
if (tune_type == RFF_UP) {
rate_ff_test_init();
step_time_limit_ms = 10000;
} else if (tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP) {
@ -187,7 +187,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
if (tune_type == SP_UP) {
angle_dwell_test_run(start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
} else if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) {
} else if (tune_type == RFF_UP) {
rate_ff_test_run(AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD, AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS, dir_sign);
} else if (tune_type == RP_UP || tune_type == RD_UP) {
dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
@ -247,7 +247,6 @@ void AC_AutoTune_Heli::do_gcs_announcements()
// 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:
case RP_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case RP_UP:
@ -259,9 +258,6 @@ void AC_AutoTune_Heli::do_gcs_announcements()
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff);
break;
case RFF_DOWN:
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);
@ -281,6 +277,12 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
{
AC_AutoTune::backup_gains_and_initialise();
// initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli
freq_cnt = 0;
start_freq = 0.0f;
stop_freq = 0.0f;
ff_up_first_iter = true;
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
@ -2061,6 +2063,17 @@ float AC_AutoTune_Heli::get_yaw_rate_filt_min() const
return AUTOTUNE_RLPF_MIN;
}
// reset the test vaariables for each vehicle
void AC_AutoTune_Heli::reset_vehicle_test_variables()
{
// reset dwell test variables if sweep was interrupted in order to restart sweep
if (!is_equal(start_freq, stop_freq)) {
freq_cnt = 0;
start_freq = 0.0f;
stop_freq = 0.0f;
}
}
// set the tuning test sequence
void AC_AutoTune_Heli::set_tune_sequence()
{

3
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -53,6 +53,9 @@ protected:
// load test gains
void load_test_gains() override;
// reset the test vaariables for heli
void reset_vehicle_test_variables() override;
// initializes test
void test_init() override;

2
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -120,11 +120,9 @@ void AC_AutoTune_Multi::do_gcs_announcements()
case RD_UP:
case RD_DOWN:
case RP_UP:
case RP_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case RFF_UP:
case RFF_DOWN:
break;
case SP_DOWN:
case SP_UP:

3
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -53,6 +53,9 @@ protected:
// load test gains
void load_test_gains() override;
// reset the test vaariables for heli
void reset_vehicle_test_variables() override {};
void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override;
void do_gcs_announcements() override;