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

AC_AutoTune: move load and backup gain methods to subclasses

This commit is contained in:
Bill Geyer 2021-12-22 09:45:38 -05:00 committed by Bill Geyer
parent cd64216558
commit 40321754fa
6 changed files with 395 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -709,180 +709,6 @@ void AC_AutoTune::backup_gains_and_initialise()
desired_yaw_cd = ahrs_view->yaw_sensor;
aggressiveness = constrain_float(aggressiveness, 0.05f, 0.2f);
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
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();
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();
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
tune_roll_accel = attitude_control->get_accel_roll_max_cdss();
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
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();
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();
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit();
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();
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();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
// called by stop and failed functions
void AC_AutoTune::load_orig_gains()
{
attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) {
if (!is_zero(orig_roll_rp) || allow_zero_rate_p()) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
}
if (pitch_enabled()) {
if (!is_zero(orig_pitch_rp) || allow_zero_rate_p()) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
}
if (yaw_enabled()) {
if (!is_zero(orig_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
}
// load_tuned_gains - load tuned gains
void AC_AutoTune::load_tuned_gains()
{
if (!attitude_control->get_bf_feedforward()) {
attitude_control->bf_feedforward(true);
attitude_control->set_accel_roll_max_cdss(0.0f);
attitude_control->set_accel_pitch_max_cdss(0.0f);
}
if (roll_enabled()) {
if (!is_zero(tune_roll_rp) || allow_zero_rate_p()) {
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().kI(get_tuned_ri(axis));
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
}
if (pitch_enabled()) {
if (!is_zero(tune_pitch_rp) || allow_zero_rate_p()) {
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(get_tuned_ri(axis));
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
}
if (yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(get_tuned_ri(axis));
attitude_control->get_rate_yaw_pid().kD(get_tuned_yaw_rd());
attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
}
}
// load_intra_test_gains - gains used between tests
// called during testing mode's update-gains step to set gains ahead of return-to-level step
void AC_AutoTune::load_intra_test_gains()
{
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
// sanity check the gains
attitude_control->bf_feedforward(true);
if (roll_enabled()) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(get_intra_test_ri(axis));
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
if (pitch_enabled()) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(get_intra_test_ri(axis));
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(get_intra_test_ri(axis));
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
/*

41
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -96,18 +96,25 @@ protected:
// log PIDs at full rate for during twitch
virtual void log_pids() = 0;
//
// methods to load and save gains
//
// backup original gains and prepare for start of tuning
virtual void backup_gains_and_initialise();
// switch to use original gains
virtual void load_orig_gains() = 0;
// switch to gains found by last successful autotune
virtual void load_tuned_gains() = 0;
// load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
virtual void load_intra_test_gains() = 0;
// load gains for next test. relies on axis variable being set
virtual void load_test_gains() = 0;
// get intra test rate I gain for the specified axis
virtual float get_intra_test_ri(AxisType test_axis) = 0;
// get tuned rate I gain for the specified axis
virtual float get_tuned_ri(AxisType test_axis) = 0;
// get tuned yaw rate d gain
virtual float get_tuned_yaw_rd() = 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;
@ -278,22 +285,6 @@ private:
// directly updates attitude controller with targets
void control_attitude();
//
// methods to load and save gains
//
// backup original gains and prepare for start of tuning
void backup_gains_and_initialise();
// switch to use original gains
void load_orig_gains();
// switch to gains found by last successful autotune
void load_tuned_gains();
// load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
void load_intra_test_gains();
// get attitude for slow position hold in autotune mode
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);

207
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -275,6 +275,177 @@ void AC_AutoTune_Heli::do_gcs_announcements()
announce_time = now;
}
// 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()
{
AC_AutoTune::backup_gains_and_initialise();
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
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();
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();
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
tune_roll_accel = attitude_control->get_accel_roll_max_cdss();
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
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();
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();
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit();
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();
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();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
// called by stop and failed functions
void AC_AutoTune_Heli::load_orig_gains()
{
attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
if (pitch_enabled()) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
// load_tuned_gains - load tuned gains
void AC_AutoTune_Heli::load_tuned_gains()
{
if (!attitude_control->get_bf_feedforward()) {
attitude_control->bf_feedforward(true);
attitude_control->set_accel_roll_max_cdss(0.0f);
attitude_control->set_accel_pitch_max_cdss(0.0f);
}
if (roll_enabled()) {
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().kI(tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
if (pitch_enabled()) {
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
if (yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
}
}
// load_intra_test_gains - gains used between tests
// called during testing mode's update-gains step to set gains ahead of return-to-level step
void AC_AutoTune_Heli::load_intra_test_gains()
{
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
// sanity check the gains
attitude_control->bf_feedforward(true);
if (roll_enabled()) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
if (pitch_enabled()) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
// load_test_gains - load the to-be-tested gains for a single axis
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli::load_test_gains()
@ -1872,42 +2043,6 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float pha
phase);
}
// get intra test rate I gain for the specified axis
float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING;
break;
case PITCH:
ret = orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING;
break;
case YAW:
ret = orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
}
return ret;
}
// get tuned rate I gain for the specified axis
float AC_AutoTune_Heli::get_tuned_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL;
break;
case PITCH:
ret = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL;
break;
case YAW:
ret = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
break;
}
return ret;
}
// get minimum rate P (for any axis)
float AC_AutoTune_Heli::get_rp_min() const
{

26
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -34,17 +34,25 @@ public:
protected:
//
// methods to load and save gains
//
// backup original gains and prepare for start of tuning
void backup_gains_and_initialise() override;
// switch to use original gains
void load_orig_gains() override;
// switch to gains found by last successful autotune
void load_tuned_gains() override;
// load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
void load_intra_test_gains() override;
// load test gains
void load_test_gains() override;
// get intra test rate I gain for the specified axis
float get_intra_test_ri(AxisType test_axis) override;
// get tuned rate I gain for the specified axis
float get_tuned_ri(AxisType test_axis) override;
// get tuned yaw rate d gain
float get_tuned_yaw_rd() override { return tune_yaw_rd; }
// initializes test
void test_init() override;

210
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -149,6 +149,180 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign)
twitch_test_run(test_axis, dir_sign);
}
// backup_gains_and_initialise - store current gains as originals
// called before tuning starts to backup original gains
void AC_AutoTune_Multi::backup_gains_and_initialise()
{
AC_AutoTune::backup_gains_and_initialise();
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
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();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
tune_roll_accel = attitude_control->get_accel_roll_max_cdss();
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
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();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit();
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();
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
// called by stop and failed functions
void AC_AutoTune_Multi::load_orig_gains()
{
attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) {
if (!is_zero(orig_roll_rp)) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
}
if (pitch_enabled()) {
if (!is_zero(orig_pitch_rp)) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
}
if (yaw_enabled()) {
if (!is_zero(orig_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
}
// load_tuned_gains - load tuned gains
void AC_AutoTune_Multi::load_tuned_gains()
{
if (!attitude_control->get_bf_feedforward()) {
attitude_control->bf_feedforward(true);
attitude_control->set_accel_roll_max_cdss(0.0f);
attitude_control->set_accel_pitch_max_cdss(0.0f);
}
if (roll_enabled()) {
if (!is_zero(tune_roll_rp)) {
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
}
if (pitch_enabled()) {
if (!is_zero(tune_pitch_rp)) {
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
}
if (yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control->get_rate_yaw_pid().kD(0.0f);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
}
}
// load_intra_test_gains - gains used between tests
// called during testing mode's update-gains step to set gains ahead of return-to-level step
void AC_AutoTune_Multi::load_intra_test_gains()
{
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
// sanity check the gains
attitude_control->bf_feedforward(true);
if (roll_enabled()) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
}
if (pitch_enabled()) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
}
if (yaw_enabled()) {
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
}
}
// load_test_gains - load the to-be-tested gains for a single axis
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Multi::load_test_gains()
@ -935,42 +1109,6 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
}
}
// get intra test rate I gain for the specified axis
float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = orig_roll_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
case PITCH:
ret = orig_pitch_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
case YAW:
ret = orig_yaw_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
}
return ret;
}
// get tuned rate I gain for the specified axis
float AC_AutoTune_Multi::get_tuned_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
case PITCH:
ret = tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
case YAW:
ret = tune_yaw_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
}
return ret;
}
// get minimum rate P (for any axis)
float AC_AutoTune_Multi::get_rp_min() const
{

27
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -34,18 +34,25 @@ public:
static const struct AP_Param::GroupInfo var_info[];
protected:
// load gains
//
// methods to load and save gains
//
// backup original gains and prepare for start of tuning
void backup_gains_and_initialise() override;
// switch to use original gains
void load_orig_gains() override;
// switch to gains found by last successful autotune
void load_tuned_gains() override;
// load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
void load_intra_test_gains() override;
// load test gains
void load_test_gains() override;
// get intra test rate I gain for the specified axis
float get_intra_test_ri(AxisType test_axis) override;
// get tuned rate I gain for the specified axis
float get_tuned_ri(AxisType test_axis) override;
// get tuned yaw rate D gain
float get_tuned_yaw_rd() override { return 0.0f; }
void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override;
void do_gcs_announcements() override;