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

AC_AutoTune: set slew rate to 0 while twitching

Browse Source
This commit is contained in:
Andy Piper 2021-11-05 20:53:11 +00:00 committed by Randy Mackay
parent bac523976e
commit 0c38b3f456
2 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -931,6 +931,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff(); orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz(); 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_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max(); orig_roll_accel = attitude_control->get_accel_roll_max();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP(); tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
@ -943,6 +944,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff(); orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz(); 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_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max(); orig_pitch_accel = attitude_control->get_accel_pitch_max();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
@ -955,6 +957,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff(); orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz(); 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_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max(); orig_yaw_accel = attitude_control->get_accel_yaw_max();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
@ -978,6 +981,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_roll_pid().kD(orig_roll_rd); 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().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); 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->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max(orig_roll_accel); attitude_control->set_accel_roll_max(orig_roll_accel);
} }
@ -989,6 +993,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); 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().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); 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->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max(orig_pitch_accel); attitude_control->set_accel_pitch_max(orig_pitch_accel);
} }
@ -1001,6 +1006,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); 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_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); 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->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max(orig_yaw_accel); attitude_control->set_accel_yaw_max(orig_yaw_accel);
} }
@ -1061,6 +1067,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_roll_pid().kD(orig_roll_rd); 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().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); 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->get_angle_roll_p().kP(orig_roll_sp);
} }
if (pitch_enabled()) { if (pitch_enabled()) {
@ -1069,6 +1076,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); 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().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); 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->get_angle_pitch_p().kP(orig_pitch_sp);
} }
if (yaw_enabled()) { if (yaw_enabled()) {
@ -1077,6 +1085,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); 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().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); 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_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
} }
@ -1093,6 +1102,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_roll_pid().kD(tune_roll_rd); attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(0.0f); attitude_control->get_rate_roll_pid().ff(0.0f);
attitude_control->get_rate_roll_pid().filt_T_hz(0.0f); attitude_control->get_rate_roll_pid().filt_T_hz(0.0f);
attitude_control->get_rate_roll_pid().slew_limit(0.0f);
attitude_control->get_angle_roll_p().kP(tune_roll_sp); attitude_control->get_angle_roll_p().kP(tune_roll_sp);
break; break;
case PITCH: case PITCH:
@ -1101,6 +1111,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(0.0f); attitude_control->get_rate_pitch_pid().ff(0.0f);
attitude_control->get_rate_pitch_pid().filt_T_hz(0.0f); attitude_control->get_rate_pitch_pid().filt_T_hz(0.0f);
attitude_control->get_rate_pitch_pid().slew_limit(0.0f);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
break; break;
case YAW: case YAW:
@ -1110,6 +1121,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_yaw_pid().ff(0.0f); attitude_control->get_rate_yaw_pid().ff(0.0f);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(0.0f); attitude_control->get_rate_yaw_pid().filt_T_hz(0.0f);
attitude_control->get_rate_yaw_pid().slew_limit(0.0f);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
break; break;
} }
@ -1159,6 +1171,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_roll_pid().kD(tune_roll_rd); attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff); 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().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_rate_roll_pid().save_gains(); attitude_control->get_rate_roll_pid().save_gains();
// stabilize roll // stabilize roll
@ -1184,6 +1197,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); 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().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_rate_pitch_pid().save_gains(); attitude_control->get_rate_pitch_pid().save_gains();
// stabilize pitch // stabilize pitch
@ -1209,6 +1223,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_yaw_pid().kD(0.0f); 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().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); 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(tune_yaw_rLPF); attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().save_gains(); attitude_control->get_rate_yaw_pid().save_gains();

6
libraries/AC_AutoTune/AC_AutoTune.h
View File

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