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

AC_AutoTune: support for upgrade to PID object

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
0bb9db5139
commit
c7196a4232
1 changed files with 10 additions and 10 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      libraries/AC_AutoTune/AC_AutoTune.cpp

20
libraries/AC_AutoTune/AC_AutoTune.cpp
Unescape View File

@ -529,7 +529,7 @@ void AC_AutoTune::control_attitude()
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor; start_angle = ahrs_view->roll_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
break; break;
case PITCH: case PITCH:
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
@ -538,7 +538,7 @@ void AC_AutoTune::control_attitude()
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor; start_angle = ahrs_view->pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break; break;
case YAW: case YAW:
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
@ -946,11 +946,11 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
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_rLPF = attitude_control->get_rate_yaw_pid().filt_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();
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); 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();
@ -988,7 +988,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); 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().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_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);
attitude_control->set_accel_yaw_max(orig_yaw_accel); attitude_control->set_accel_yaw_max(orig_yaw_accel);
} }
@ -1029,7 +1029,7 @@ void AC_AutoTune::load_tuned_gains()
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); 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().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_hz(tune_yaw_rLPF); attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); 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(tune_yaw_accel);
} }
@ -1062,7 +1062,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); 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().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_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);
} }
} }
@ -1091,7 +1091,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); 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().kD(0.0f);
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_hz(tune_yaw_rLPF); attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
break; break;
} }
@ -1191,7 +1191,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); 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().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_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();
// stabilize yaw // stabilize yaw
@ -1206,7 +1206,7 @@ void AC_AutoTune::save_tuning_gains()
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
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_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
orig_yaw_accel = attitude_control->get_accel_pitch_max(); orig_yaw_accel = attitude_control->get_accel_pitch_max();
} }

Write Preview
Loading…
Cancel
Save