Skip to content

Commit

Permalink
AC_AutoTune: rate and accel limits
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 11, 2024
1 parent dadb634 commit 725a28a
Showing 1 changed file with 57 additions and 27 deletions.
84 changes: 57 additions & 27 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,14 +109,14 @@ const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {
// @Description: maximum angular acceleration in deg/s/s allowed during autotune maneuvers
// @Range: 1 4000
// @User: Standard
AP_GROUPINFO("ACC_MAX", 7, AC_AutoTune_Heli, accel_max, 5000.0f),
AP_GROUPINFO("ACC_MAX", 7, AC_AutoTune_Heli, accel_max, 0.0f),

// @Param: RAT_MAX
// @DisplayName: Autotune maximum allowable angular rate
// @Description: maximum angular rate in deg/s allowed during autotune maneuvers
// @Range: 0 500
// @User: Standard
AP_GROUPINFO("RAT_MAX", 8, AC_AutoTune_Heli, rate_max, 50.0f),
AP_GROUPINFO("RAT_MAX", 8, AC_AutoTune_Heli, rate_max, 0.0f),

AP_GROUPEND
};
Expand Down Expand Up @@ -519,11 +519,21 @@ void AC_AutoTune_Heli::load_intra_test_gains()
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli::load_test_gains()
{
float rate_p, rate_i, rate_d, rate_test_max;
float rate_p, rate_i, rate_d, rate_test_max, accel_test_max;
switch (axis) {
case ROLL:
rate_test_max = orig_roll_rate;
accel_test_max = tune_roll_accel;
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL;
// have attitude controller use rate limit if non-zero
if (is_positive(rate_max)) {
rate_test_max = rate_max;
}
// have attitude controller use accel limit if non-zero
if (is_positive(accel_max)) {
accel_test_max = accel_max;
}
} else {
// freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_roll_ri;
Expand All @@ -535,18 +545,21 @@ void AC_AutoTune_Heli::load_test_gains()
rate_p = tune_roll_rp;
rate_d = tune_roll_rd;
}
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_roll_rate;
}
*/
rate_test_max = orig_roll_rate;
load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f, rate_test_max);
load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, accel_test_max, orig_roll_fltt, 0.0f, 0.0f, rate_test_max);
break;
case PITCH:
rate_test_max = orig_pitch_rate;
accel_test_max = tune_pitch_accel;
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL;
// have attitude controller use rate limit if non-zero
if (is_positive(rate_max)) {
rate_test_max = rate_max;
}
// have attitude controller use accel limit if non-zero
if (is_positive(accel_max)) {
accel_test_max = accel_max;
}
} else {
// freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_pitch_ri;
Expand All @@ -558,31 +571,27 @@ void AC_AutoTune_Heli::load_test_gains()
rate_p = tune_pitch_rp;
rate_d = tune_pitch_rd;
}
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_pitch_rate;
}
*/
rate_test_max = orig_pitch_rate;
load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, 0.0f, rate_test_max);
load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, accel_test_max, orig_pitch_fltt, 0.0f, 0.0f, rate_test_max);
break;
case YAW:
case YAW_D:
rate_test_max = orig_yaw_rate;
accel_test_max = tune_yaw_accel;
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
// have attitude controller use rate limit if non-zero
if (is_positive(rate_max)) {
rate_test_max = rate_max;
}
// have attitude controller use accel limit if non-zero
if (is_positive(accel_max)) {
accel_test_max = accel_max;
}
} else {
// freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_yaw_ri;
}
/* if (tune_type == RFF_UP) {
rate_test_max = 5.0f;
} else {
rate_test_max = orig_yaw_rate;
}
*/
rate_test_max = orig_yaw_rate;
load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f, rate_test_max);
load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, accel_test_max, orig_yaw_fltt, tune_yaw_rLPF, 0.0f, rate_test_max);
break;
}
}
Expand Down Expand Up @@ -810,6 +819,27 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
if (settle_time == 0) {
target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, tgt_attitude * 5730.0f);
dwell_freq = chirp_input.get_frequency_rads();
// limit requested angle based on calculated max accel and max rate if non zero parameters are set
if (test_calc_type == RATE) {
// apply rate limit for RFF, RP_UP and RD_UP test types
float peak_rate = dwell_freq * tgt_attitude;
float peak_accel = sq(dwell_freq) * tgt_attitude;
float rate_knockdown = 1.0f;
float accel_knockdown = 1.0f;
// reduce target angle by ratio of peak rate to max rate
if (is_positive(rate_max) && peak_rate > rate_max) {
rate_knockdown = rate_max / peak_rate;
}
// reduce target angle by ratio of peak accel to max accel
if (is_positive(accel_max) && peak_accel > accel_max) {
accel_knockdown = accel_max / peak_accel;
}
if (rate_knockdown < accel_knockdown) {
target_angle_cd *= rate_knockdown;
} else if (accel_knockdown < rate_knockdown) {
target_angle_cd *= accel_knockdown;
}
}
const Vector2f att_fdbk {
-5730.0f * vel_hold_gain * velocity_bf.y,
5730.0f * vel_hold_gain * velocity_bf.x
Expand Down

0 comments on commit 725a28a

Please sign in to comment.