Browse Source

AC_AutoTune: support for upgrade to PID object

mission-4.1.18
Leonard Hall 6 years ago committed by Randy Mackay
parent
commit
c7196a4232
  1. 20
      libraries/AC_AutoTune/AC_AutoTune.cpp

20
libraries/AC_AutoTune/AC_AutoTune.cpp

@ -529,7 +529,7 @@ void AC_AutoTune::control_attitude() @@ -529,7 +529,7 @@ void AC_AutoTune::control_attitude()
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
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;
case PITCH:
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() @@ -538,7 +538,7 @@ void AC_AutoTune::control_attitude()
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
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;
case YAW:
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() @@ -946,11 +946,11 @@ void AC_AutoTune::backup_gains_and_initialise()
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_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max();
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_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_accel = attitude_control->get_accel_yaw_max();
@ -988,7 +988,7 @@ void AC_AutoTune::load_orig_gains() @@ -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().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_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max(orig_yaw_accel);
}
@ -1029,7 +1029,7 @@ void AC_AutoTune::load_tuned_gains() @@ -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().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_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);
}
@ -1062,7 +1062,7 @@ void AC_AutoTune::load_intra_test_gains() @@ -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().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_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
}
}
@ -1091,7 +1091,7 @@ void AC_AutoTune::load_twitch_gains() @@ -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().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_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
break;
}
@ -1191,7 +1191,7 @@ void AC_AutoTune::save_tuning_gains() @@ -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().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_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().save_gains();
// stabilize yaw
@ -1206,7 +1206,7 @@ void AC_AutoTune::save_tuning_gains() @@ -1206,7 +1206,7 @@ void AC_AutoTune::save_tuning_gains()
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_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_pitch_max();
}

Loading…
Cancel
Save