Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: move heli specific methods to sub class

Browse Source
This commit is contained in:
Bill Geyer 2021-12-12 23:02:40 -05:00 committed by Bill Geyer
parent da947d4498
commit 92cfd3fc63
4 changed files with 781 additions and 782 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

686
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -1038,689 +1038,3 @@ void AC_AutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out,
yaw_cd_out = target_yaw_cd;
}
void AC_AutoTune::rate_ff_test_init()
{
ff_test_phase = 0;
rotation_rate_filt.reset(0);
rotation_rate_filt.set_cutoff_frequency(5.0f);
command_filt.reset(0);
command_filt.set_cutoff_frequency(5.0f);
target_rate_filt.reset(0);
target_rate_filt.set_cutoff_frequency(5.0f);
test_command_filt = 0.0f;
test_rate_filt = 0.0f;
test_tgt_rate_filt = 0.0f;
filt_target_rate = 0.0f;
settle_time = 200;
phase_out_time = 500;
}
void AC_AutoTune::rate_ff_test_run(float max_angle_cd, float target_rate_cds, float dir_sign)
{
float gyro_reading = 0.0f;
float command_reading =0.0f;
float tgt_rate_reading = 0.0f;
const uint32_t now = AP_HAL::millis();
// TODO make filter leak dependent on dt
const float filt_alpha = 0.0123f;
target_rate_cds = dir_sign * target_rate_cds;
switch (axis) {
case ROLL:
gyro_reading = ahrs_view->get_gyro().x;
command_reading = motors->get_roll();
tgt_rate_reading = attitude_control->rate_bf_targets().x;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_roll();
rate_request_cds = gyro_reading;
} else if (((ahrs_view->roll_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
} else if (((ahrs_view->roll_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
attitude_control->rate_bf_roll_target(rate_request_cds);
} else if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
attitude_control->rate_bf_roll_target(rate_request_cds);
} else if (((ahrs_view->roll_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().x;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f);
} else if (ff_test_phase == 2 ) {
angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f);
phase_out_time--;
}
break;
case PITCH:
gyro_reading = ahrs_view->get_gyro().y;
command_reading = motors->get_pitch();
tgt_rate_reading = attitude_control->rate_bf_targets().y;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_pitch();
rate_request_cds = gyro_reading;
} else if (((ahrs_view->pitch_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
} else if (((ahrs_view->pitch_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
attitude_control->rate_bf_pitch_target(rate_request_cds);
} else if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
attitude_control->rate_bf_pitch_target(rate_request_cds);
} else if (((ahrs_view->pitch_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().y;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f);
} else if (ff_test_phase == 2 ) {
angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f);
phase_out_time--;
}
break;
case YAW:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_yaw();
trim_heading = ahrs_view->yaw_sensor;
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
attitude_control->rate_bf_yaw_target(rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
attitude_control->rate_bf_yaw_target(rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().z;
attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false);
} else if (ff_test_phase == 2 ) {
angle_request_cd += wrap_180_cd(trim_heading - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false);
}
break;
}
rotation_rate = rotation_rate_filt.apply(gyro_reading,
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - trim_command_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply(tgt_rate_reading,
AP::scheduler().get_loop_period_s());
// record steady state rate and motor command
switch (axis) {
case ROLL:
if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
case PITCH:
if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
case YAW:
if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
}
if (now - step_start_time_ms >= step_time_limit_ms || (ff_test_phase == 2 && phase_out_time == 0)) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
rate_request_cds = 0.0f;
angle_request_cd = 0.0f;
}
}
void AC_AutoTune::dwell_test_init(float filt_freq)
{
rotation_rate_filt.reset(0);
rotation_rate_filt.set_cutoff_frequency(filt_freq);
command_filt.reset(0);
command_filt.set_cutoff_frequency(filt_freq);
target_rate_filt.reset(0);
target_rate_filt.set_cutoff_frequency(filt_freq);
filt_target_rate = 0.0f;
dwell_start_time_ms = 0.0f;
settle_time = 200;
if (!is_equal(start_freq, stop_freq)) {
sweep.ph180_freq = 0.0f;
sweep.ph180_gain = 0.0f;
sweep.ph180_phase = 0.0f;
sweep.ph270_freq = 0.0f;
sweep.ph270_gain = 0.0f;
sweep.ph270_phase = 0.0f;
sweep.maxgain_gain = 0.0f;
sweep.maxgain_freq = 0.0f;
sweep.maxgain_phase = 0.0f;
sweep.progress = 0;
curr_test_gain = 0.0f;
curr_test_phase = 0.0f;
}
// save the trim output from PID controller
float ff_term = 0.0f;
float p_term = 0.0f;
switch (axis) {
case ROLL:
trim_meas_rate = ahrs_view->get_gyro().x;
ff_term = attitude_control->get_rate_roll_pid().get_ff();
p_term = attitude_control->get_rate_roll_pid().get_p();
break;
case PITCH:
trim_meas_rate = ahrs_view->get_gyro().y;
ff_term = attitude_control->get_rate_pitch_pid().get_ff();
p_term = attitude_control->get_rate_pitch_pid().get_p();
break;
case YAW:
trim_meas_rate = ahrs_view->get_gyro().z;
ff_term = attitude_control->get_rate_yaw_pid().get_ff();
p_term = attitude_control->get_rate_yaw_pid().get_p();
break;
}
trim_pff_out = ff_term + p_term;
}
void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
float tgt_rate_reading = 0.0f;
float tgt_attitude = 2.5f * 0.01745f;
const uint32_t now = AP_HAL::millis();
float target_rate_cds;
float sweep_time_ms = 23000;
const float att_hold_gain = 4.5f;
Vector3f attitude_cd;
float dwell_freq = start_frq;
float cycle_time_ms = 0;
if (!is_zero(dwell_freq)) {
cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq;
}
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
}
// keep controller from requesting too high of a rate
float target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f;
if (target_rate_mag_cds > 5000.0f) {
target_rate_mag_cds = 5000.0f;
}
if (settle_time == 0) {
// give gentler start for the dwell
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
target_rate_cds = -0.5f * target_rate_mag_cds * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001);
} else {
if (is_equal(start_frq,stop_frq)) {
target_rate_cds = - target_rate_mag_cds * cosf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001);
} else {
target_rate_cds = waveform((now - dwell_start_time_ms - 0.5f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.5f * cycle_time_ms) * 0.001f, target_rate_mag_cds, start_frq, stop_frq);
dwell_freq = waveform_freq_rads;
}
}
filt_attitude_cd.x += alpha * (attitude_cd.x - filt_attitude_cd.x);
filt_attitude_cd.y += alpha * (attitude_cd.y - filt_attitude_cd.y);
filt_attitude_cd.z += alpha * wrap_180_cd(attitude_cd.z - filt_attitude_cd.z);
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
} else {
target_rate_cds = 0.0f;
settle_time--;
dwell_start_time_ms = now;
trim_command = command_out;
filt_attitude_cd = attitude_cd;
trim_attitude_cd = attitude_cd;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
}
// limit rate correction for position hold
Vector3f trim_rate_cds;
trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x);
trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y);
trim_rate_cds.z = att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.z);
trim_rate_cds.x = constrain_float(trim_rate_cds.x, -15000.0f, 15000.0f);
trim_rate_cds.y = constrain_float(trim_rate_cds.y, -15000.0f, 15000.0f);
trim_rate_cds.z = constrain_float(trim_rate_cds.z, -15000.0f, 15000.0f);
switch (axis) {
case ROLL:
gyro_reading = ahrs_view->get_gyro().x;
command_reading = motors->get_roll();
tgt_rate_reading = attitude_control->rate_bf_targets().x;
if (settle_time == 0) {
float ff_rate_contr = 0.0f;
if (tune_roll_rff > 0.0f) {
ff_rate_contr = 5730.0f * trim_command / tune_roll_rff;
}
trim_rate_cds.x += ff_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, trim_rate_cds.y, 0.0f);
attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds.x);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_roll_pid().kP()) / (attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP());
attitude_control->rate_bf_roll_target(trim_tgt_rate_cds);
}
}
break;
case PITCH:
gyro_reading = ahrs_view->get_gyro().y;
command_reading = motors->get_pitch();
tgt_rate_reading = attitude_control->rate_bf_targets().y;
if (settle_time == 0) {
float ff_rate_contr = 0.0f;
if (tune_pitch_rff > 0.0f) {
ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff;
}
trim_rate_cds.y += ff_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, 0.0f, 0.0f);
attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds.y);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_pitch_pid().kP()) / (attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP());
attitude_control->rate_bf_pitch_target(trim_tgt_rate_cds);
}
}
break;
case YAW:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
if (settle_time == 0) {
float rp_rate_contr = 0.0f;
if (tune_yaw_rp > 0.0f) {
rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp;
}
trim_rate_cds.z += rp_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, trim_rate_cds.y, 0.0f);
attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds.z);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_yaw_pid().kP()) / (attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP());
attitude_control->rate_bf_yaw_target(trim_tgt_rate_cds);
}
}
break;
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase or just start if sweep
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
if (freq_resp_input == 1) {
freqresp_rate.update_rate(filt_target_rate,rotation_rate, dwell_freq);
} else {
freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq);
}
if (freqresp_rate.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_rate.get_freq();
curr_test_gain = freqresp_rate.get_gain();
curr_test_phase = freqresp_rate.get_phase();
// reset cycle_complete to allow indication of next cycle
freqresp_rate.reset_cycle_complete();
// log sweep data
Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else {
dwell_gain = freqresp_rate.get_gain();
dwell_phase = freqresp_rate.get_phase();
}
}
}
// set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) {
// track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped.
if (curr_test_phase > 180.0f && sweep.progress == 0) {
sweep.progress = 1;
} else if (curr_test_phase > 270.0f && sweep.progress == 1) {
sweep.progress = 2;
}
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f && sweep.progress == 0) {
sweep.ph180_freq = curr_test_freq;
sweep.ph180_gain = curr_test_gain;
sweep.ph180_phase = curr_test_phase;
}
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f && sweep.progress == 1) {
sweep.ph270_freq = curr_test_freq;
sweep.ph270_gain = curr_test_gain;
sweep.ph270_phase = curr_test_phase;
}
if (curr_test_gain > sweep.maxgain_gain) {
sweep.maxgain_gain = curr_test_gain;
sweep.maxgain_freq = curr_test_freq;
sweep.maxgain_phase = curr_test_phase;
}
if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
}
} else {
if (now - step_start_time_ms >= step_time_limit_ms || freqresp_rate.is_cycle_complete()) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
}
}
void AC_AutoTune::angle_dwell_test_init(float filt_freq)
{
rotation_rate_filt.set_cutoff_frequency(filt_freq);
command_filt.set_cutoff_frequency(filt_freq);
target_rate_filt.set_cutoff_frequency(filt_freq);
dwell_start_time_ms = 0.0f;
settle_time = 200;
switch (axis) {
case ROLL:
rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f);
command_filt.reset(motors->get_roll());
target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f);
rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f;
command_out = motors->get_roll();
filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
break;
case PITCH:
rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f);
command_filt.reset(motors->get_pitch());
target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f);
rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f;
command_out = motors->get_pitch();
filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
break;
case YAW:
// yaw angle will be centered on zero by removing trim heading
rotation_rate_filt.reset(0.0f);
command_filt.reset(motors->get_yaw());
target_rate_filt.reset(0.0f);
rotation_rate = 0.0f;
command_out = motors->get_yaw();
filt_target_rate = 0.0f;
break;
}
if (!is_equal(start_freq, stop_freq)) {
sweep.ph180_freq = 0.0f;
sweep.ph180_gain = 0.0f;
sweep.ph180_phase = 0.0f;
sweep.ph270_freq = 0.0f;
sweep.ph270_gain = 0.0f;
sweep.ph270_phase = 0.0f;
sweep.maxgain_gain = 0.0f;
sweep.maxgain_freq = 0.0f;
sweep.maxgain_phase = 0.0f;
curr_test_gain = 0.0f;
curr_test_phase = 0.0f;
}
}
void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
float tgt_rate_reading = 0.0f;
float tgt_attitude = 5.0f * 0.01745f;
const uint32_t now = AP_HAL::millis();
float target_angle_cd;
float sweep_time_ms = 23000;
float dwell_freq = start_frq;
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
// adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized
const float freq_co = 1.0f / attitude_control->get_input_tc();
const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f;
tgt_attitude = constrain_float(0.08725f * (1.0f + 0.2f * added_ampl), 0.08725f, 0.5235f);
float cycle_time_ms = 0;
if (!is_zero(dwell_freq)) {
cycle_time_ms = 1000.0f * 6.28f / dwell_freq;
}
// body frame calculation of velocity
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
}
if (settle_time == 0) {
// give gentler start for the dwell
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
target_angle_cd = 0.5f * tgt_attitude * 5730.0f * (cosf(dwell_freq * (now - dwell_start_time_ms) * 0.001) - 1.0f);
} else {
if (is_equal(start_frq,stop_frq)) {
target_angle_cd = -tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001);
} else {
target_angle_cd = -waveform((now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.25f * cycle_time_ms) * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq);
dwell_freq = waveform_freq_rads;
}
}
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
filt_att_fdbk_from_velxy_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.x, -2000.0f, 2000.0f);
filt_att_fdbk_from_velxy_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.y, -2000.0f, 2000.0f);
} else {
target_angle_cd = 0.0f;
trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z;
trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor;
settle_time--;
dwell_start_time_ms = now;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
}
switch (axis) {
case ROLL:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, 0.0f);
command_reading = motors->get_roll();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f;
break;
case PITCH:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(filt_att_fdbk_from_velxy_cd.x, target_angle_cd + filt_att_fdbk_from_velxy_cd.y, 0.0f);
command_reading = motors->get_pitch();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f;
break;
case YAW:
command_reading = motors->get_yaw();
tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f;
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f;
attitude_control->input_euler_angle_roll_pitch_yaw(filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
break;
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
freqresp_angle.update_angle(command_out, filt_target_rate, rotation_rate, dwell_freq);
if (freqresp_angle.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_angle.get_freq();
curr_test_gain = freqresp_angle.get_gain();
curr_test_phase = freqresp_angle.get_phase();
test_accel_max = freqresp_angle.get_accel_max();
// reset cycle_complete to allow indication of next cycle
freqresp_angle.reset_cycle_complete();
// log sweep data
Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else {
dwell_gain = freqresp_angle.get_gain();
dwell_phase = freqresp_angle.get_phase();
test_accel_max = freqresp_angle.get_accel_max();
}
}
}
// set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq)) {
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f) {
sweep.ph180_freq = curr_test_freq;
sweep.ph180_gain = curr_test_gain;
sweep.ph180_phase = curr_test_phase;
}
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f) {
sweep.ph270_freq = curr_test_freq;
sweep.ph270_gain = curr_test_gain;
sweep.ph270_phase = curr_test_phase;
}
if (curr_test_gain > sweep.maxgain_gain) {
sweep.maxgain_gain = curr_test_gain;
sweep.maxgain_freq = curr_test_freq;
sweep.maxgain_phase = curr_test_phase;
}
if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
} else {
if (now - step_start_time_ms >= step_time_limit_ms || freqresp_angle.is_cycle_complete()) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
}
}
// init_test - initialises the test
float AC_AutoTune::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax)
{
float time_fade_in = 0.0f; // Time to reach maximum amplitude of chirp
float time_fade_out = 0.1 * time_record; // Time to reach zero amplitude after chirp finishes
float time_const_freq = 0.0f;
float window;
float output;
float B = logf(wMax / wMin);
if (time <= 0.0f) {
window = 0.0f;
} else if (time <= time_fade_in) {
window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in);
} else if (time <= time_record - time_fade_out) {
window = 1.0;
} else if (time <= time_record) {
window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI);
} else {
window = 0.0;
}
if (time <= 0.0f) {
waveform_freq_rads = wMin;
output = 0.0f;
} else if (time <= time_const_freq) {
waveform_freq_rads = wMin;
output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq);
} else if (time <= time_record) {
waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq));
output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1));
} else {
waveform_freq_rads = wMax;
output = 0.0f;
}
return output;
}

97
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -346,103 +346,8 @@ protected:
// generic method used by subclasses to update gains for the max gain tune type
virtual void updating_max_gains_all(AxisType test_axis)=0;
// Feedforward test used to determine Rate FF gain
void rate_ff_test_init();
void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_init(float filt_freq);
void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// dwell test used to perform frequency dwells for angle gains
void angle_dwell_test_init(float filt_freq);
void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// generates waveform for frequency sweep excitations
float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
uint8_t ff_test_phase; // phase of feedforward test
float test_command_filt; // filtered commanded output for FF test analysis
float test_rate_filt; // filtered rate output for FF test analysis
float command_out; // test axis command output
float test_tgt_rate_filt; // filtered target rate for FF test analysis
float filt_target_rate; // filtered target rate
bool ff_up_first_iter; // true on first iteration of ff up testing
float test_gain[20]; // frequency response gain for each dwell test iteration
float test_freq[20]; // frequency of each dwell test iteration
float test_phase[20]; // frequency response phase for each dwell test iteration
float dwell_start_time_ms; // start time in ms of dwell test
uint8_t freq_cnt; // dwell test iteration counter
uint8_t freq_cnt_max; // counter number for frequency that produced max gain response
float curr_test_freq; // current test frequency
float curr_test_gain; // current test frequency response gain
float curr_test_phase; // current test frequency response phase
Vector3f start_angles; // aircraft attitude at the start of test
uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test
uint32_t phase_out_time; // time in ms to phase out response
float waveform_freq_rads; //current frequency for chirp waveform
float start_freq; //start freq for dwell test
float stop_freq; //ending freq for dwell test
float trim_pff_out; // trim output of the PID rate controller for P, I and FF terms
float trim_meas_rate; // trim measured gyro rate
//variables from rate FF test
float trim_command_reading;
float trim_heading;
float rate_request_cds;
float angle_request_cd;
// variables from rate dwell test
Vector3f trim_attitude_cd;
Vector3f filt_attitude_cd;
Vector2f filt_att_fdbk_from_velxy_cd;
float filt_command_reading;
float filt_gyro_reading;
float filt_tgt_rate_reading;
float trim_command;
// variables from angle dwell test
float trim_yaw_tgt_reading;
float trim_yaw_heading_reading;
// Vector2f filt_att_fdbk_from_velxy_cd;
// float filt_command_reading;
// float filt_gyro_reading;
// float filt_tgt_rate_reading;
LowPassFilterFloat command_filt; // filtered command
LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second
// sweep_data tracks the overall characteristics in the response to the frequency sweep
struct sweep_data {
float maxgain_freq;
float maxgain_gain;
float maxgain_phase;
float ph180_freq;
float ph180_gain;
float ph180_phase;
float ph270_freq;
float ph270_gain;
float ph270_phase;
uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
};
sweep_data sweep;
// max_gain_data type stores information from the max gain test
struct max_gain_data {
float freq;
float phase;
float gain;
float max_allowed;
};
// max gain data for rate p tuning
max_gain_data max_rate_p;
// max gain data for rate d tuning
max_gain_data max_rate_d;
// freqresp object for the rate frequency response tests
AC_AutoTune_FreqResp freqresp_rate;
// freqresp object for the angle frequency response tests
AC_AutoTune_FreqResp freqresp_angle;
bool ff_up_first_iter; // true on first iteration of ff up testing
};

685
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -435,6 +435,691 @@ void AC_AutoTune_Heli::save_tuning_gains()
reset();
}
void AC_AutoTune_Heli::rate_ff_test_init()
{
ff_test_phase = 0;
rotation_rate_filt.reset(0);
rotation_rate_filt.set_cutoff_frequency(5.0f);
command_filt.reset(0);
command_filt.set_cutoff_frequency(5.0f);
target_rate_filt.reset(0);
target_rate_filt.set_cutoff_frequency(5.0f);
test_command_filt = 0.0f;
test_rate_filt = 0.0f;
test_tgt_rate_filt = 0.0f;
filt_target_rate = 0.0f;
settle_time = 200;
phase_out_time = 500;
}
void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cds, float dir_sign)
{
float gyro_reading = 0.0f;
float command_reading =0.0f;
float tgt_rate_reading = 0.0f;
const uint32_t now = AP_HAL::millis();
// TODO make filter leak dependent on dt
const float filt_alpha = 0.0123f;
target_rate_cds = dir_sign * target_rate_cds;
switch (axis) {
case ROLL:
gyro_reading = ahrs_view->get_gyro().x;
command_reading = motors->get_roll();
tgt_rate_reading = attitude_control->rate_bf_targets().x;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_roll();
rate_request_cds = gyro_reading;
} else if (((ahrs_view->roll_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
} else if (((ahrs_view->roll_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
attitude_control->rate_bf_roll_target(rate_request_cds);
} else if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f);
attitude_control->rate_bf_roll_target(rate_request_cds);
} else if (((ahrs_view->roll_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().x;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f);
} else if (ff_test_phase == 2 ) {
angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f);
phase_out_time--;
}
break;
case PITCH:
gyro_reading = ahrs_view->get_gyro().y;
command_reading = motors->get_pitch();
tgt_rate_reading = attitude_control->rate_bf_targets().y;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_pitch();
rate_request_cds = gyro_reading;
} else if (((ahrs_view->pitch_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
} else if (((ahrs_view->pitch_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
attitude_control->rate_bf_pitch_target(rate_request_cds);
} else if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f);
attitude_control->rate_bf_pitch_target(rate_request_cds);
} else if (((ahrs_view->pitch_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().y;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f);
} else if (ff_test_phase == 2 ) {
angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f);
phase_out_time--;
}
break;
case YAW:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
if (settle_time > 0) {
settle_time--;
trim_command_reading = motors->get_yaw();
trim_heading = ahrs_view->yaw_sensor;
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 0) {
ff_test_phase = 1;
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
attitude_control->rate_bf_yaw_target(rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds);
attitude_control->rate_bf_yaw_target(rate_request_cds);
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
ff_test_phase = 2;
angle_request_cd = attitude_control->get_att_target_euler_cd().z;
attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false);
} else if (ff_test_phase == 2 ) {
angle_request_cd += wrap_180_cd(trim_heading - angle_request_cd) * filt_alpha;
attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false);
}
break;
}
rotation_rate = rotation_rate_filt.apply(gyro_reading,
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - trim_command_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply(tgt_rate_reading,
AP::scheduler().get_loop_period_s());
// record steady state rate and motor command
switch (axis) {
case ROLL:
if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
case PITCH:
if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
case YAW:
if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
&& ff_test_phase == 1 ) {
test_rate_filt = rotation_rate;
test_command_filt = command_out;
test_tgt_rate_filt = filt_target_rate;
}
break;
}
if (now - step_start_time_ms >= step_time_limit_ms || (ff_test_phase == 2 && phase_out_time == 0)) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
rate_request_cds = 0.0f;
angle_request_cd = 0.0f;
}
}
void AC_AutoTune_Heli::dwell_test_init(float filt_freq)
{
rotation_rate_filt.reset(0);
rotation_rate_filt.set_cutoff_frequency(filt_freq);
command_filt.reset(0);
command_filt.set_cutoff_frequency(filt_freq);
target_rate_filt.reset(0);
target_rate_filt.set_cutoff_frequency(filt_freq);
filt_target_rate = 0.0f;
dwell_start_time_ms = 0.0f;
settle_time = 200;
if (!is_equal(start_freq, stop_freq)) {
sweep.ph180_freq = 0.0f;
sweep.ph180_gain = 0.0f;
sweep.ph180_phase = 0.0f;
sweep.ph270_freq = 0.0f;
sweep.ph270_gain = 0.0f;
sweep.ph270_phase = 0.0f;
sweep.maxgain_gain = 0.0f;
sweep.maxgain_freq = 0.0f;
sweep.maxgain_phase = 0.0f;
sweep.progress = 0;
curr_test_gain = 0.0f;
curr_test_phase = 0.0f;
}
// save the trim output from PID controller
float ff_term = 0.0f;
float p_term = 0.0f;
switch (axis) {
case ROLL:
trim_meas_rate = ahrs_view->get_gyro().x;
ff_term = attitude_control->get_rate_roll_pid().get_ff();
p_term = attitude_control->get_rate_roll_pid().get_p();
break;
case PITCH:
trim_meas_rate = ahrs_view->get_gyro().y;
ff_term = attitude_control->get_rate_pitch_pid().get_ff();
p_term = attitude_control->get_rate_pitch_pid().get_p();
break;
case YAW:
trim_meas_rate = ahrs_view->get_gyro().z;
ff_term = attitude_control->get_rate_yaw_pid().get_ff();
p_term = attitude_control->get_rate_yaw_pid().get_p();
break;
}
trim_pff_out = ff_term + p_term;
}
void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
float tgt_rate_reading = 0.0f;
float tgt_attitude = 2.5f * 0.01745f;
const uint32_t now = AP_HAL::millis();
float target_rate_cds;
float sweep_time_ms = 23000;
const float att_hold_gain = 4.5f;
Vector3f attitude_cd;
float dwell_freq = start_frq;
float cycle_time_ms = 0;
if (!is_zero(dwell_freq)) {
cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq;
}
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
}
// keep controller from requesting too high of a rate
float target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f;
if (target_rate_mag_cds > 5000.0f) {
target_rate_mag_cds = 5000.0f;
}
if (settle_time == 0) {
// give gentler start for the dwell
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
target_rate_cds = -0.5f * target_rate_mag_cds * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001);
} else {
if (is_equal(start_frq,stop_frq)) {
target_rate_cds = - target_rate_mag_cds * cosf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001);
} else {
target_rate_cds = waveform((now - dwell_start_time_ms - 0.5f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.5f * cycle_time_ms) * 0.001f, target_rate_mag_cds, start_frq, stop_frq);
dwell_freq = waveform_freq_rads;
}
}
filt_attitude_cd.x += alpha * (attitude_cd.x - filt_attitude_cd.x);
filt_attitude_cd.y += alpha * (attitude_cd.y - filt_attitude_cd.y);
filt_attitude_cd.z += alpha * wrap_180_cd(attitude_cd.z - filt_attitude_cd.z);
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
} else {
target_rate_cds = 0.0f;
settle_time--;
dwell_start_time_ms = now;
trim_command = command_out;
filt_attitude_cd = attitude_cd;
trim_attitude_cd = attitude_cd;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
}
// limit rate correction for position hold
Vector3f trim_rate_cds;
trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x);
trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y);
trim_rate_cds.z = att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.z);
trim_rate_cds.x = constrain_float(trim_rate_cds.x, -15000.0f, 15000.0f);
trim_rate_cds.y = constrain_float(trim_rate_cds.y, -15000.0f, 15000.0f);
trim_rate_cds.z = constrain_float(trim_rate_cds.z, -15000.0f, 15000.0f);
switch (axis) {
case ROLL:
gyro_reading = ahrs_view->get_gyro().x;
command_reading = motors->get_roll();
tgt_rate_reading = attitude_control->rate_bf_targets().x;
if (settle_time == 0) {
float ff_rate_contr = 0.0f;
if (tune_roll_rff > 0.0f) {
ff_rate_contr = 5730.0f * trim_command / tune_roll_rff;
}
trim_rate_cds.x += ff_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, trim_rate_cds.y, 0.0f);
attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds.x);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_roll_pid().kP()) / (attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP());
attitude_control->rate_bf_roll_target(trim_tgt_rate_cds);
}
}
break;
case PITCH:
gyro_reading = ahrs_view->get_gyro().y;
command_reading = motors->get_pitch();
tgt_rate_reading = attitude_control->rate_bf_targets().y;
if (settle_time == 0) {
float ff_rate_contr = 0.0f;
if (tune_pitch_rff > 0.0f) {
ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff;
}
trim_rate_cds.y += ff_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, 0.0f, 0.0f);
attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds.y);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_pitch_pid().kP()) / (attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP());
attitude_control->rate_bf_pitch_target(trim_tgt_rate_cds);
}
}
break;
case YAW:
gyro_reading = ahrs_view->get_gyro().z;
command_reading = motors->get_yaw();
tgt_rate_reading = attitude_control->rate_bf_targets().z;
if (settle_time == 0) {
float rp_rate_contr = 0.0f;
if (tune_yaw_rp > 0.0f) {
rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp;
}
trim_rate_cds.z += rp_rate_contr;
attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, trim_rate_cds.y, 0.0f);
attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds.z);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if (!is_zero(attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP())) {
float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_yaw_pid().kP()) / (attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP());
attitude_control->rate_bf_yaw_target(trim_tgt_rate_cds);
}
}
break;
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase or just start if sweep
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
if (freq_resp_input == 1) {
freqresp_rate.update_rate(filt_target_rate,rotation_rate, dwell_freq);
} else {
freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq);
}
if (freqresp_rate.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_rate.get_freq();
curr_test_gain = freqresp_rate.get_gain();
curr_test_phase = freqresp_rate.get_phase();
// reset cycle_complete to allow indication of next cycle
freqresp_rate.reset_cycle_complete();
// log sweep data
Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else {
dwell_gain = freqresp_rate.get_gain();
dwell_phase = freqresp_rate.get_phase();
}
}
}
// set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) {
// track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped.
if (curr_test_phase > 180.0f && sweep.progress == 0) {
sweep.progress = 1;
} else if (curr_test_phase > 270.0f && sweep.progress == 1) {
sweep.progress = 2;
}
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f && sweep.progress == 0) {
sweep.ph180_freq = curr_test_freq;
sweep.ph180_gain = curr_test_gain;
sweep.ph180_phase = curr_test_phase;
}
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f && sweep.progress == 1) {
sweep.ph270_freq = curr_test_freq;
sweep.ph270_gain = curr_test_gain;
sweep.ph270_phase = curr_test_phase;
}
if (curr_test_gain > sweep.maxgain_gain) {
sweep.maxgain_gain = curr_test_gain;
sweep.maxgain_freq = curr_test_freq;
sweep.maxgain_phase = curr_test_phase;
}
if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain));
}
} else {
if (now - step_start_time_ms >= step_time_limit_ms || freqresp_rate.is_cycle_complete()) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
}
}
void AC_AutoTune_Heli::angle_dwell_test_init(float filt_freq)
{
rotation_rate_filt.set_cutoff_frequency(filt_freq);
command_filt.set_cutoff_frequency(filt_freq);
target_rate_filt.set_cutoff_frequency(filt_freq);
dwell_start_time_ms = 0.0f;
settle_time = 200;
switch (axis) {
case ROLL:
rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f);
command_filt.reset(motors->get_roll());
target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f);
rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f;
command_out = motors->get_roll();
filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
break;
case PITCH:
rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f);
command_filt.reset(motors->get_pitch());
target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f);
rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f;
command_out = motors->get_pitch();
filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
break;
case YAW:
// yaw angle will be centered on zero by removing trim heading
rotation_rate_filt.reset(0.0f);
command_filt.reset(motors->get_yaw());
target_rate_filt.reset(0.0f);
rotation_rate = 0.0f;
command_out = motors->get_yaw();
filt_target_rate = 0.0f;
break;
}
if (!is_equal(start_freq, stop_freq)) {
sweep.ph180_freq = 0.0f;
sweep.ph180_gain = 0.0f;
sweep.ph180_phase = 0.0f;
sweep.ph270_freq = 0.0f;
sweep.ph270_gain = 0.0f;
sweep.ph270_phase = 0.0f;
sweep.maxgain_gain = 0.0f;
sweep.maxgain_freq = 0.0f;
sweep.maxgain_phase = 0.0f;
curr_test_gain = 0.0f;
curr_test_phase = 0.0f;
}
}
void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
float tgt_rate_reading = 0.0f;
float tgt_attitude = 5.0f * 0.01745f;
const uint32_t now = AP_HAL::millis();
float target_angle_cd;
float sweep_time_ms = 23000;
float dwell_freq = start_frq;
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
// adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized
const float freq_co = 1.0f / attitude_control->get_input_tc();
const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f;
tgt_attitude = constrain_float(0.08725f * (1.0f + 0.2f * added_ampl), 0.08725f, 0.5235f);
float cycle_time_ms = 0;
if (!is_zero(dwell_freq)) {
cycle_time_ms = 1000.0f * 6.28f / dwell_freq;
}
// body frame calculation of velocity
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
}
if (settle_time == 0) {
// give gentler start for the dwell
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
target_angle_cd = 0.5f * tgt_attitude * 5730.0f * (cosf(dwell_freq * (now - dwell_start_time_ms) * 0.001) - 1.0f);
} else {
if (is_equal(start_frq,stop_frq)) {
target_angle_cd = -tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001);
} else {
target_angle_cd = -waveform((now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.25f * cycle_time_ms) * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq);
dwell_freq = waveform_freq_rads;
}
}
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
filt_att_fdbk_from_velxy_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.x, -2000.0f, 2000.0f);
filt_att_fdbk_from_velxy_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.y, -2000.0f, 2000.0f);
} else {
target_angle_cd = 0.0f;
trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z;
trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor;
settle_time--;
dwell_start_time_ms = now;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
}
switch (axis) {
case ROLL:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, 0.0f);
command_reading = motors->get_roll();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f;
break;
case PITCH:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(filt_att_fdbk_from_velxy_cd.x, target_angle_cd + filt_att_fdbk_from_velxy_cd.y, 0.0f);
command_reading = motors->get_pitch();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f;
break;
case YAW:
command_reading = motors->get_yaw();
tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f;
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f;
attitude_control->input_euler_angle_roll_pitch_yaw(filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
break;
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
freqresp_angle.update_angle(command_out, filt_target_rate, rotation_rate, dwell_freq);
if (freqresp_angle.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_angle.get_freq();
curr_test_gain = freqresp_angle.get_gain();
curr_test_phase = freqresp_angle.get_phase();
test_accel_max = freqresp_angle.get_accel_max();
// reset cycle_complete to allow indication of next cycle
freqresp_angle.reset_cycle_complete();
// log sweep data
Log_AutoTuneSweep();
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
} else {
dwell_gain = freqresp_angle.get_gain();
dwell_phase = freqresp_angle.get_phase();
test_accel_max = freqresp_angle.get_accel_max();
}
}
}
// set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq)) {
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f) {
sweep.ph180_freq = curr_test_freq;
sweep.ph180_gain = curr_test_gain;
sweep.ph180_phase = curr_test_phase;
}
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f) {
sweep.ph270_freq = curr_test_freq;
sweep.ph270_gain = curr_test_gain;
sweep.ph270_phase = curr_test_phase;
}
if (curr_test_gain > sweep.maxgain_gain) {
sweep.maxgain_gain = curr_test_gain;
sweep.maxgain_freq = curr_test_freq;
sweep.maxgain_phase = curr_test_phase;
}
if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
} else {
if (now - step_start_time_ms >= step_time_limit_ms || freqresp_angle.is_cycle_complete()) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
}
}
// init_test - initialises the test
float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax)
{
float time_fade_in = 0.0f; // Time to reach maximum amplitude of chirp
float time_fade_out = 0.1 * time_record; // Time to reach zero amplitude after chirp finishes
float time_const_freq = 0.0f;
float window;
float output;
float B = logf(wMax / wMin);
if (time <= 0.0f) {
window = 0.0f;
} else if (time <= time_fade_in) {
window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in);
} else if (time <= time_record - time_fade_out) {
window = 1.0;
} else if (time <= time_record) {
window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI);
} else {
window = 0.0;
}
if (time <= 0.0f) {
waveform_freq_rads = wMin;
output = 0.0f;
} else if (time <= time_const_freq) {
waveform_freq_rads = wMin;
output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq);
} else if (time <= time_record) {
waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq));
output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1));
} else {
waveform_freq_rads = wMax;
output = 0.0f;
}
return output;
}
// update gains for the rate p up tune type
void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
{

95
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -121,6 +121,34 @@ protected:
AP_Float max_resp_gain;
private:
// max_gain_data type stores information from the max gain test
struct max_gain_data {
float freq;
float phase;
float gain;
float max_allowed;
};
// max gain data for rate p tuning
max_gain_data max_rate_p;
// max gain data for rate d tuning
max_gain_data max_rate_d;
// Feedforward test used to determine Rate FF gain
void rate_ff_test_init();
void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_init(float filt_freq);
void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// dwell test used to perform frequency dwells for angle gains
void angle_dwell_test_init(float filt_freq);
void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// generates waveform for frequency sweep excitations
float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command);
@ -180,4 +208,71 @@ private:
// previous gain
float rd_prev_gain;
uint8_t ff_test_phase; // phase of feedforward test
float test_command_filt; // filtered commanded output for FF test analysis
float test_rate_filt; // filtered rate output for FF test analysis
float command_out; // test axis command output
float test_tgt_rate_filt; // filtered target rate for FF test analysis
float filt_target_rate; // filtered target rate
float test_gain[20]; // frequency response gain for each dwell test iteration
float test_freq[20]; // frequency of each dwell test iteration
float test_phase[20]; // frequency response phase for each dwell test iteration
float dwell_start_time_ms; // start time in ms of dwell test
uint8_t freq_cnt_max; // counter number for frequency that produced max gain response
float curr_test_freq; // current test frequency
float curr_test_gain; // current test frequency response gain
float curr_test_phase; // current test frequency response phase
Vector3f start_angles; // aircraft attitude at the start of test
uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test
uint32_t phase_out_time; // time in ms to phase out response
float waveform_freq_rads; //current frequency for chirp waveform
float trim_pff_out; // trim output of the PID rate controller for P, I and FF terms
float trim_meas_rate; // trim measured gyro rate
//variables from rate FF test
float trim_command_reading;
float trim_heading;
float rate_request_cds;
float angle_request_cd;
// variables from rate dwell test
Vector3f trim_attitude_cd;
Vector3f filt_attitude_cd;
Vector2f filt_att_fdbk_from_velxy_cd;
float filt_command_reading;
float filt_gyro_reading;
float filt_tgt_rate_reading;
float trim_command;
// variables from angle dwell test
float trim_yaw_tgt_reading;
float trim_yaw_heading_reading;
// Vector2f filt_att_fdbk_from_velxy_cd;
// float filt_command_reading;
// float filt_gyro_reading;
// float filt_tgt_rate_reading;
LowPassFilterFloat command_filt; // filtered command
LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second
// sweep_data tracks the overall characteristics in the response to the frequency sweep
struct sweep_data {
float maxgain_freq;
float maxgain_gain;
float maxgain_phase;
float ph180_freq;
float ph180_gain;
float ph180_phase;
float ph270_freq;
float ph270_gain;
float ph270_phase;
uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
};
sweep_data sweep;
// freqresp object for the rate frequency response tests
AC_AutoTune_FreqResp freqresp_rate;
// freqresp object for the angle frequency response tests
AC_AutoTune_FreqResp freqresp_angle;
};