mirror of https://github.com/ArduPilot/ardupilot
AC_AutoTune: limit autotune target rates to parameter limits
this makes it safer to autotune a large vehicle which may become unstable at very high target rates
This commit is contained in:
parent
ad7caa39c3
commit
96793c43c7
|
@ -527,8 +527,12 @@ void AC_AutoTune::control_attitude()
|
||||||
|
|
||||||
float target_max_rate;
|
float target_max_rate;
|
||||||
switch (axis) {
|
switch (axis) {
|
||||||
case ROLL:
|
case ROLL: {
|
||||||
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);
|
||||||
|
const float roll_rate_max_cds = degrees(attitude_control->get_ang_vel_roll_max_rads()) * 100;
|
||||||
|
if (is_positive(roll_rate_max_cds)) {
|
||||||
|
target_max_rate = MIN(target_max_rate, roll_rate_max_cds);
|
||||||
|
}
|
||||||
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
|
||||||
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||||
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
|
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
|
||||||
|
@ -536,8 +540,13 @@ void AC_AutoTune::control_attitude()
|
||||||
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_D_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);
|
||||||
|
const float pitch_rate_max_cds = degrees(attitude_control->get_ang_vel_pitch_max_rads()) * 100;
|
||||||
|
if (is_positive(pitch_rate_max_cds)) {
|
||||||
|
target_max_rate = MIN(target_max_rate, pitch_rate_max_cds);
|
||||||
|
}
|
||||||
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
|
||||||
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||||
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
|
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
|
||||||
|
@ -545,8 +554,13 @@ void AC_AutoTune::control_attitude()
|
||||||
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_D_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);
|
||||||
|
const float yaw_rate_max_cds = degrees(attitude_control->get_ang_vel_yaw_max_rads()) * 100;
|
||||||
|
if (is_positive(yaw_rate_max_cds)) {
|
||||||
|
target_max_rate = MIN(target_max_rate, yaw_rate_max_cds);
|
||||||
|
}
|
||||||
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
|
||||||
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
|
||||||
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
|
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
|
||||||
|
@ -555,6 +569,7 @@ void AC_AutoTune::control_attitude()
|
||||||
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
|
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
||||||
rotation_rate_filt.reset(start_rate);
|
rotation_rate_filt.reset(start_rate);
|
||||||
} else {
|
} else {
|
||||||
|
|
Loading…
Reference in New Issue