|
|
|
@ -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(); |
|
|
|
|
} |
|
|
|
|