From 92cfd3fc6326f454d409472fe0c4b7577b5f67c3 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sun, 12 Dec 2021 23:02:40 -0500 Subject: [PATCH] AC_AutoTune: move heli specific methods to sub class --- libraries/AC_AutoTune/AC_AutoTune.cpp | 686 --------------------- libraries/AC_AutoTune/AC_AutoTune.h | 97 +-- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 685 ++++++++++++++++++++ libraries/AC_AutoTune/AC_AutoTune_Heli.h | 95 +++ 4 files changed, 781 insertions(+), 782 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 424a871b9b..581770ee88 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -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; -} - diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 98f0f34d70..8eb65cfe3e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -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 }; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 9a55743134..9da56ddf63 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -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) { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index b0f01c1c19..2a546c1432 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -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; + };