mirror of https://github.com/ArduPilot/ardupilot
AC_AutoTune: combine dwell_run_test for angle and rate
This commit is contained in:
parent
4680f73715
commit
74db41806b
|
@ -155,11 +155,11 @@ void AC_AutoTune_Heli::test_init()
|
||||||
}
|
}
|
||||||
if (!is_equal(start_freq,stop_freq)) {
|
if (!is_equal(start_freq,stop_freq)) {
|
||||||
// initialize determine_gain function whenever test is initialized
|
// initialize determine_gain function whenever test is initialized
|
||||||
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE);
|
freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE);
|
||||||
dwell_test_init(start_freq, stop_freq, RATE);
|
dwell_test_init(start_freq, stop_freq, RATE);
|
||||||
} else {
|
} else {
|
||||||
// initialize determine_gain function whenever test is initialized
|
// initialize determine_gain function whenever test is initialized
|
||||||
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE);
|
freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE);
|
||||||
dwell_test_init(start_freq, start_freq, RATE);
|
dwell_test_init(start_freq, start_freq, RATE);
|
||||||
}
|
}
|
||||||
if (!is_zero(start_freq)) {
|
if (!is_zero(start_freq)) {
|
||||||
|
@ -185,11 +185,11 @@ void AC_AutoTune_Heli::test_init()
|
||||||
|
|
||||||
if (!is_equal(start_freq,stop_freq)) {
|
if (!is_equal(start_freq,stop_freq)) {
|
||||||
// initialize determine gain function
|
// initialize determine gain function
|
||||||
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE);
|
freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE);
|
||||||
dwell_test_init(start_freq, stop_freq, ANGLE);
|
dwell_test_init(start_freq, stop_freq, ANGLE);
|
||||||
} else {
|
} else {
|
||||||
// initialize determine gain function
|
// initialize determine gain function
|
||||||
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE);
|
freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE);
|
||||||
dwell_test_init(start_freq, start_freq, ANGLE);
|
dwell_test_init(start_freq, start_freq, ANGLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -246,13 +246,13 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
|
||||||
break;
|
break;
|
||||||
case RP_UP:
|
case RP_UP:
|
||||||
case RD_UP:
|
case RD_UP:
|
||||||
dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
|
dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], RATE);
|
||||||
break;
|
break;
|
||||||
case MAX_GAINS:
|
case MAX_GAINS:
|
||||||
dwell_test_run(0, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
|
dwell_test_run(0, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], RATE);
|
||||||
break;
|
break;
|
||||||
case SP_UP:
|
case SP_UP:
|
||||||
angle_dwell_test_run(start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
|
dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], ANGLE);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
step = UPDATE_GAINS;
|
step = UPDATE_GAINS;
|
||||||
|
@ -996,70 +996,86 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq, DwellTy
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase)
|
void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type)
|
||||||
{
|
{
|
||||||
float gyro_reading = 0.0f;
|
float gyro_reading = 0.0f;
|
||||||
float command_reading = 0.0f;
|
float command_reading = 0.0f;
|
||||||
float tgt_rate_reading = 0.0f;
|
float tgt_rate_reading = 0.0f;
|
||||||
float tgt_attitude = 2.5f * 0.01745f;
|
float tgt_attitude;
|
||||||
const uint32_t now = AP_HAL::millis();
|
const uint32_t now = AP_HAL::millis();
|
||||||
|
float target_angle_cd;
|
||||||
float target_rate_cds;
|
float target_rate_cds;
|
||||||
float sweep_time_ms = 23000;
|
float sweep_time_ms = 23000;
|
||||||
const float att_hold_gain = 4.5f;
|
|
||||||
Vector3f attitude_cd;
|
|
||||||
|
|
||||||
float dwell_freq = start_frq;
|
float dwell_freq = start_frq;
|
||||||
|
float target_rate_mag_cds;
|
||||||
|
const float att_hold_gain = 4.5f;
|
||||||
|
|
||||||
float cycle_time_ms = 0;
|
float cycle_time_ms = 0;
|
||||||
if (!is_zero(dwell_freq)) {
|
if (!is_zero(dwell_freq)) {
|
||||||
cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq;
|
cycle_time_ms = 1000.0f * M_2PI / dwell_freq;
|
||||||
}
|
}
|
||||||
|
|
||||||
attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
|
if (dwell_type == RATE) {
|
||||||
|
// keep controller from requesting too high of a rate
|
||||||
|
tgt_attitude = 2.5f * 0.01745f;
|
||||||
|
target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f;
|
||||||
|
if (target_rate_mag_cds > 5000.0f) {
|
||||||
|
target_rate_mag_cds = 5000.0f;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
tgt_attitude = 5.0f * 0.01745f;
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// body frame calculation of velocity
|
||||||
Vector3f velocity_ned, velocity_bf;
|
Vector3f velocity_ned, velocity_bf;
|
||||||
if (ahrs_view->get_velocity_NED(velocity_ned)) {
|
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.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();
|
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
|
Vector3f attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
|
||||||
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) {
|
if (settle_time == 0) {
|
||||||
// give gentler start for the dwell
|
if (dwell_type == RATE) {
|
||||||
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
|
target_rate_cds = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, target_rate_mag_cds, start_frq, stop_frq);
|
||||||
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_pit_roll_cd.apply(Vector2f(attitude_cd.x,attitude_cd.y), AP::scheduler().get_loop_period_s());
|
filt_pit_roll_cd.apply(Vector2f(attitude_cd.x,attitude_cd.y), AP::scheduler().get_loop_period_s());
|
||||||
filt_heading_error_cd.apply(wrap_180_cd(trim_attitude_cd.z - attitude_cd.z), AP::scheduler().get_loop_period_s());
|
filt_heading_error_cd.apply(wrap_180_cd(trim_attitude_cd.z - attitude_cd.z), AP::scheduler().get_loop_period_s());
|
||||||
Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x);
|
|
||||||
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
|
|
||||||
} else {
|
} else {
|
||||||
|
target_angle_cd = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq);
|
||||||
|
}
|
||||||
|
const Vector2f att_fdbk {
|
||||||
|
-5730.0f * vel_hold_gain * velocity_bf.y,
|
||||||
|
5730.0f * vel_hold_gain * velocity_bf.x
|
||||||
|
};
|
||||||
|
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
|
||||||
|
dwell_freq = waveform_freq_rads;
|
||||||
|
} else {
|
||||||
|
if (dwell_type == RATE) {
|
||||||
target_rate_cds = 0.0f;
|
target_rate_cds = 0.0f;
|
||||||
settle_time--;
|
|
||||||
dwell_start_time_ms = now;
|
|
||||||
trim_command = command_out;
|
trim_command = command_out;
|
||||||
trim_attitude_cd = attitude_cd;
|
trim_attitude_cd = attitude_cd;
|
||||||
filt_pit_roll_cd.reset(Vector2f(attitude_cd.x,attitude_cd.y));
|
filt_pit_roll_cd.reset(Vector2f(attitude_cd.x,attitude_cd.y));
|
||||||
filt_heading_error_cd.reset(0.0f);
|
filt_heading_error_cd.reset(0.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;
|
||||||
|
}
|
||||||
|
dwell_start_time_ms = now;
|
||||||
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
|
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
|
||||||
|
settle_time--;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (dwell_type == RATE) {
|
||||||
// limit rate correction for position hold
|
// limit rate correction for position hold
|
||||||
Vector3f trim_rate_cds {
|
Vector3f trim_rate_cds {
|
||||||
constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f),
|
constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f),
|
||||||
constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f),
|
constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f),
|
||||||
constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f)
|
constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f)
|
||||||
};
|
};
|
||||||
|
|
||||||
switch (axis) {
|
switch (axis) {
|
||||||
case ROLL:
|
case ROLL:
|
||||||
gyro_reading = ahrs_view->get_gyro().x;
|
gyro_reading = ahrs_view->get_gyro().x;
|
||||||
|
@ -1122,139 +1138,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (settle_time == 0) {
|
|
||||||
filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
|
|
||||||
filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s());
|
|
||||||
filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s());
|
|
||||||
} else {
|
} else {
|
||||||
filt_command_reading.reset(command_reading);
|
|
||||||
filt_gyro_reading.reset(gyro_reading);
|
|
||||||
filt_tgt_rate_reading.reset(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.get()),
|
|
||||||
AP::scheduler().get_loop_period_s());
|
|
||||||
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()),
|
|
||||||
AP::scheduler().get_loop_period_s());
|
|
||||||
command_out = command_filt.apply((command_reading - filt_command_reading.get()),
|
|
||||||
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(command_out,filt_target_rate,rotation_rate, dwell_freq);
|
|
||||||
} else {
|
|
||||||
freqresp_rate.update(command_out,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();
|
|
||||||
} 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
} 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_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;
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
const Vector2f att_fdbk {
|
|
||||||
-5730.0f * vel_hold_gain * velocity_bf.y,
|
|
||||||
5730.0f * vel_hold_gain * velocity_bf.x
|
|
||||||
};
|
|
||||||
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
|
|
||||||
} 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.reset(Vector2f(0.0f,0.0f));
|
|
||||||
}
|
|
||||||
|
|
||||||
const Vector2f trim_angle_cd {
|
const Vector2f trim_angle_cd {
|
||||||
constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f),
|
constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f),
|
||||||
constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f)
|
constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f)
|
||||||
|
@ -1279,6 +1163,7 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
|
||||||
attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
|
attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (settle_time == 0) {
|
if (settle_time == 0) {
|
||||||
filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
|
filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
|
||||||
|
@ -1300,31 +1185,42 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
|
||||||
|
|
||||||
// wait for dwell to start before determining gain and phase
|
// 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)) {
|
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
|
||||||
freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq);
|
if (freq_resp_input == 1) {
|
||||||
if (freqresp_angle.is_cycle_complete()) {
|
freqresp.update(command_out,filt_target_rate,rotation_rate, dwell_freq);
|
||||||
|
} else {
|
||||||
|
freqresp.update(command_out,command_out,rotation_rate, dwell_freq);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (freqresp.is_cycle_complete()) {
|
||||||
if (!is_equal(start_frq,stop_frq)) {
|
if (!is_equal(start_frq,stop_frq)) {
|
||||||
curr_test.freq = freqresp_angle.get_freq();
|
curr_test.freq = freqresp.get_freq();
|
||||||
curr_test.gain = freqresp_angle.get_gain();
|
curr_test.gain = freqresp.get_gain();
|
||||||
curr_test.phase = freqresp_angle.get_phase();
|
curr_test.phase = freqresp.get_phase();
|
||||||
test_accel_max = freqresp_angle.get_accel_max();
|
if (dwell_type == ANGLE) {test_accel_max = freqresp.get_accel_max();}
|
||||||
// reset cycle_complete to allow indication of next cycle
|
// reset cycle_complete to allow indication of next cycle
|
||||||
freqresp_angle.reset_cycle_complete();
|
freqresp.reset_cycle_complete();
|
||||||
// log sweep data
|
// log sweep data
|
||||||
Log_AutoTuneSweep();
|
Log_AutoTuneSweep();
|
||||||
} else {
|
} else {
|
||||||
dwell_gain = freqresp_angle.get_gain();
|
dwell_gain = freqresp.get_gain();
|
||||||
dwell_phase = freqresp_angle.get_phase();
|
dwell_phase = freqresp.get_phase();
|
||||||
test_accel_max = freqresp_angle.get_accel_max();
|
if (dwell_type == ANGLE) {test_accel_max = freqresp.get_accel_max();}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// set sweep data if a frequency sweep is being conducted
|
// set sweep data if a frequency sweep is being conducted
|
||||||
if (!is_equal(start_frq,stop_frq)) {
|
if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) {
|
||||||
if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f) {
|
// 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 = curr_test;
|
sweep.ph180 = curr_test;
|
||||||
}
|
}
|
||||||
if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f) {
|
if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) {
|
||||||
sweep.ph270 = curr_test;
|
sweep.ph270 = curr_test;
|
||||||
}
|
}
|
||||||
if (curr_test.gain > sweep.maxgain.gain) {
|
if (curr_test.gain > sweep.maxgain.gain) {
|
||||||
|
@ -1335,13 +1231,12 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
|
||||||
step = UPDATE_GAINS;
|
step = UPDATE_GAINS;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
if (now - step_start_time_ms >= step_time_limit_ms || freqresp_angle.is_cycle_complete()) {
|
if (now - step_start_time_ms >= step_time_limit_ms || freqresp.is_cycle_complete()) {
|
||||||
// we have passed the maximum stop time
|
// we have passed the maximum stop time
|
||||||
step = UPDATE_GAINS;
|
step = UPDATE_GAINS;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// init_test - initialises the test
|
// init_test - initialises the test
|
||||||
float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax)
|
float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax)
|
||||||
{
|
{
|
||||||
|
|
|
@ -150,10 +150,7 @@ private:
|
||||||
void dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type);
|
void dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type);
|
||||||
|
|
||||||
// dwell test used to perform frequency dwells for rate gains
|
// dwell test used to perform frequency dwells for rate gains
|
||||||
void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
|
void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type);
|
||||||
|
|
||||||
// dwell test used to perform frequency dwells for angle gains
|
|
||||||
void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
|
|
||||||
|
|
||||||
// generates waveform for frequency sweep excitations
|
// generates waveform for frequency sweep excitations
|
||||||
float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
|
float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
|
||||||
|
@ -292,8 +289,6 @@ private:
|
||||||
AP_Float max_resp_gain; // maximum response gain
|
AP_Float max_resp_gain; // maximum response gain
|
||||||
AP_Float vel_hold_gain; // gain for velocity hold
|
AP_Float vel_hold_gain; // gain for velocity hold
|
||||||
|
|
||||||
// freqresp object for the rate frequency response tests
|
// freqresp object for the frequency response tests
|
||||||
AC_AutoTune_FreqResp freqresp_rate;
|
AC_AutoTune_FreqResp freqresp;
|
||||||
// freqresp object for the angle frequency response tests
|
|
||||||
AC_AutoTune_FreqResp freqresp_angle;
|
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue