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

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
c415-sdk
Andrew Tridgell 3 years ago
parent
3233528e37
commit
8c09da0ee6
1 changed files with 18 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      libraries/AC_AutoTune/AC_AutoTune.cpp

21
libraries/AC_AutoTune/AC_AutoTune.cpp
Unescape View File

@ -527,8 +527,12 @@ void AC_AutoTune::control_attitude() @@ -527,8 +527,12 @@ void AC_AutoTune::control_attitude()
float target_max_rate;
switch (axis) {
case ROLL:
case ROLL: {
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_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;
@ -536,8 +540,13 @@ void AC_AutoTune::control_attitude() @@ -536,8 +540,13 @@ void AC_AutoTune::control_attitude()
start_angle = ahrs_view->roll_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
break;
case PITCH:
}
case PITCH: {
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_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;
@ -545,8 +554,13 @@ void AC_AutoTune::control_attitude() @@ -545,8 +554,13 @@ void AC_AutoTune::control_attitude()
start_angle = ahrs_view->pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
case YAW:
}
case YAW: {
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_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;
@ -555,6 +569,7 @@ void AC_AutoTune::control_attitude() @@ -555,6 +569,7 @@ void AC_AutoTune::control_attitude()
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
break;
}
}
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {

Write Preview
Loading…
Cancel
Save