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

AC_AutoTune: Add units to the AC_AttitudeControl Library

This commit is contained in:
Leonard Hall 2021-09-06 19:15:45 +09:30 committed by Randy Mackay
parent f3dc47ce3d
commit 90b5bc6a2d
1 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -932,11 +932,11 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
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_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();
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();
@ -944,11 +944,11 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
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_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();
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();
@ -956,12 +956,12 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_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_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();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
}
@ -979,7 +979,7 @@ void AC_AutoTune::load_orig_gains()
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_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max(orig_roll_accel);
attitude_control->set_accel_roll_max_cdss(orig_roll_accel);
}
}
if (pitch_enabled()) {
@ -990,7 +990,7 @@ void AC_AutoTune::load_orig_gains()
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_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max(orig_pitch_accel);
attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel);
}
}
if (yaw_enabled()) {
@ -1002,7 +1002,7 @@ void AC_AutoTune::load_orig_gains()
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_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max(orig_yaw_accel);
attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel);
}
}
}
@ -1012,8 +1012,8 @@ void AC_AutoTune::load_tuned_gains()
{
if (!attitude_control->get_bf_feedforward()) {
attitude_control->bf_feedforward(true);
attitude_control->set_accel_roll_max(0.0f);
attitude_control->set_accel_pitch_max(0.0f);
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)) {
@ -1022,7 +1022,7 @@ void AC_AutoTune::load_tuned_gains()
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);
attitude_control->set_accel_roll_max_cdss(tune_roll_accel);
}
}
if (pitch_enabled()) {
@ -1032,7 +1032,7 @@ void AC_AutoTune::load_tuned_gains()
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);
attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel);
}
}
if (yaw_enabled()) {
@ -1043,7 +1043,7 @@ void AC_AutoTune::load_tuned_gains()
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(tune_yaw_accel);
attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel);
}
}
}
@ -1147,8 +1147,8 @@ void AC_AutoTune::save_tuning_gains()
if (!attitude_control->get_bf_feedforward()) {
attitude_control->bf_feedforward_save(true);
attitude_control->save_accel_roll_max(0.0f);
attitude_control->save_accel_pitch_max(0.0f);
attitude_control->save_accel_roll_max_cdss(0.0f);
attitude_control->save_accel_pitch_max_cdss(0.0f);
}
// sanity check the rate P values
@ -1166,7 +1166,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_angle_roll_p().save_gains();
// acceleration roll
attitude_control->save_accel_roll_max(tune_roll_accel);
attitude_control->save_accel_roll_max_cdss(tune_roll_accel);
// resave pids to originals in case the autotune is run again
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
@ -1174,7 +1174,7 @@ void AC_AutoTune::save_tuning_gains()
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();
orig_roll_accel = attitude_control->get_accel_roll_max_cdss();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
@ -1191,7 +1191,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_angle_pitch_p().save_gains();
// acceleration pitch
attitude_control->save_accel_pitch_max(tune_pitch_accel);
attitude_control->save_accel_pitch_max_cdss(tune_pitch_accel);
// resave pids to originals in case the autotune is run again
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
@ -1199,7 +1199,7 @@ void AC_AutoTune::save_tuning_gains()
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();
orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
@ -1217,7 +1217,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_angle_yaw_p().save_gains();
// acceleration yaw
attitude_control->save_accel_yaw_max(tune_yaw_accel);
attitude_control->save_accel_yaw_max_cdss(tune_yaw_accel);
// resave pids to originals in case the autotune is run again
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
@ -1226,7 +1226,7 @@ void AC_AutoTune::save_tuning_gains()
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
orig_yaw_accel = attitude_control->get_accel_yaw_max();
orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
}
// update GCS and log save gains event