Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune_Heli: fix rate and accel limiting

This commit is contained in:
Bill Geyer 2024-12-01 13:13:29 -05:00 committed by bnsgeyer
parent 2feee53a37
commit 8c44e565bd
1 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -528,9 +528,9 @@ void AC_AutoTune_Heli::load_test_gains()
rate_test_max = orig_roll_rate;
accel_test_max = tune_roll_accel;
} else {
// have attitude controller use accel and rate limit parameter
rate_test_max = rate_max;
accel_test_max = accel_max;
// have attitude controller not perform rate limiting and angle P scaling based on accel max
rate_test_max = 0.0;
accel_test_max = 0.0;
}
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL;
@ -552,9 +552,9 @@ void AC_AutoTune_Heli::load_test_gains()
rate_test_max = orig_pitch_rate;
accel_test_max = tune_pitch_accel;
} else {
// have attitude controller use accel and rate limit parameter
rate_test_max = rate_max;
accel_test_max = accel_max;
// have attitude controller not perform rate limiting and angle P scaling based on accel max
rate_test_max = 0.0;
accel_test_max = 0.0;
}
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL;
@ -577,9 +577,9 @@ void AC_AutoTune_Heli::load_test_gains()
rate_test_max = orig_yaw_rate;
accel_test_max = tune_yaw_accel;
} else {
// have attitude controller use accel and rate limit parameter
rate_test_max = rate_max;
accel_test_max = accel_max;
// have attitude controller not perform rate limiting and angle P scaling based on accel max
rate_test_max = 0.0;
accel_test_max = 0.0;
}
if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
@ -811,7 +811,22 @@ 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, degrees(tgt_attitude) * 100.0f);
dwell_freq = chirp_input.get_frequency_rads();
float tgt_att_limited = tgt_attitude;
if (is_positive(dwell_freq)) {
float tgt_att_temp = tgt_attitude;
if (is_positive(rate_max)) {
float ang_limit_rate = radians(rate_max) / dwell_freq;
tgt_att_temp = MIN(ang_limit_rate, tgt_attitude);
}
if (is_positive(accel_max)) {
float ang_limit_accel = radians(accel_max) / sq(dwell_freq);
tgt_att_limited = MIN(ang_limit_accel, tgt_att_temp);
} else {
tgt_att_limited = tgt_att_temp;
}
}
target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, degrees(tgt_att_limited) * 100.0f);
dwell_freq = chirp_input.get_frequency_rads();
const Vector2f att_fdbk {
-5730.0f * vel_hold_gain * velocity_bf.y,