zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_Autotune: Include FF variable

master
Leonard Hall 6 years ago committed by Andrew Tridgell
parent
b0f3674eee
commit
b25f7a38e8
2 changed files with 21 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      libraries/AC_AutoTune/AC_AutoTune.cpp
  2. 6
      libraries/AC_AutoTune/AC_AutoTune.h

18
libraries/AC_AutoTune/AC_AutoTune.cpp
Unescape View File

@ -897,6 +897,7 @@ void AC_AutoTune::backup_gains_and_initialise() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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
Unescape View File

@ -183,9 +183,9 @@ private: @@ -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

Write Preview
Loading…
Cancel
Save