diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 7e875f29f3..48374f7a0e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -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(); diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index e6fac83090..acef8dccb5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -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