Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Autotune: Include FF variable

Browse Source
This commit is contained in:
Leonard Hall 2018-12-21 18:51:59 +10:30 committed by Andrew Tridgell
parent b0f3674eee
commit b25f7a38e8
2 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -897,6 +897,7 @@ void AC_AutoTune::backup_gains_and_initialise()
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_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
@ -907,6 +908,7 @@ void AC_AutoTune::backup_gains_and_initialise()
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_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
@ -917,6 +919,7 @@ void AC_AutoTune::backup_gains_and_initialise()
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_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
@ -938,6 +941,7 @@ void AC_AutoTune::load_orig_gains()
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_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max(orig_roll_accel);
}
@ -947,6 +951,7 @@ void AC_AutoTune::load_orig_gains()
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_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max(orig_pitch_accel);
}
@ -956,6 +961,7 @@ void AC_AutoTune::load_orig_gains()
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_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max(orig_yaw_accel);
@ -976,6 +982,7 @@ void AC_AutoTune::load_tuned_gains()
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(tune_roll_accel);
}
@ -985,6 +992,7 @@ void AC_AutoTune::load_tuned_gains()
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(tune_pitch_accel);
}
@ -994,6 +1002,7 @@ void AC_AutoTune::load_tuned_gains()
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_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control->set_accel_yaw_max(tune_yaw_accel);
@ -1012,18 +1021,21 @@ void AC_AutoTune::load_intra_test_gains()
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_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_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_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
}
@ -1038,18 +1050,21 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f);
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(0.0f);
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
break;
case PITCH:
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(0.0f);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
break;
case YAW:
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
attitude_control->get_rate_yaw_pid().kD(0.0f);
attitude_control->get_rate_yaw_pid().ff(0.0f);
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
break;
@ -1090,6 +1105,7 @@ void AC_AutoTune::save_tuning_gains()
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_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max();
}
@ -1112,6 +1128,7 @@ void AC_AutoTune::save_tuning_gains()
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_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max();
}
@ -1135,6 +1152,7 @@ void AC_AutoTune::save_tuning_gains()
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_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
orig_yaw_accel = attitude_control->get_accel_pitch_max();

6
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -183,9 +183,9 @@ private:
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
bool orig_bf_feedforward;
// currently being tuned parameter values