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

AC_AutoTune: reworked updating max gains

This commit is contained in:
bnsgeyer 2024-04-13 01:04:33 -04:00 committed by Bill Geyer
parent 989f48f97a
commit ad58f3e1f8
2 changed files with 95 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

179
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -162,21 +162,17 @@ void AC_AutoTune_Heli::test_init()
num_dwell_cycles = 3;
break;
case MAX_GAINS:
if (is_zero(start_freq)) {
if (!is_zero(sweep_mtr.ph180.freq)) {
freq_cnt = 12;
test_freq[freq_cnt] = sweep_mtr.ph180.freq - 0.25f * 3.14159f * 2.0f;
curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
if (tune_type == MAX_GAINS) {
reset_maxgains_update_gain_variables();
}
} else {
start_freq = min_sweep_freq;
stop_freq = max_sweep_freq;
}
// initialize start frequency for sweep
if (!is_positive(next_test_freq)) {
start_freq = min_sweep_freq;
stop_freq = max_sweep_freq;
sweep_complete = true;
} else {
start_freq = next_test_freq;
stop_freq = start_freq;
test_accel_max = 0.0f;
}
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
@ -299,7 +295,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
return;
}
dwell_test_run(test_gain[freq_cnt], test_phase[freq_cnt], curr_data);
dwell_test_run(curr_data);
}
@ -407,7 +403,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
AC_AutoTune::backup_gains_and_initialise();
// initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli
freq_cnt = 0;
next_test_freq = 0.0f;
start_freq = 0.0f;
stop_freq = 0.0f;
@ -759,7 +755,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi
}
void AC_AutoTune_Heli::dwell_test_run(float &dwell_gain, float &dwell_phase, sweep_info &test_data)
void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
@ -924,8 +920,6 @@ void AC_AutoTune_Heli::dwell_test_run(float &dwell_gain, float &dwell_phase, swe
test_data.freq = test_start_freq;
test_data.gain = dwell_gain_tgt;
test_data.phase = dwell_phase_tgt;
dwell_gain = dwell_gain_tgt;
dwell_phase = dwell_phase_tgt;
}
} else {
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
@ -934,8 +928,6 @@ void AC_AutoTune_Heli::dwell_test_run(float &dwell_gain, float &dwell_phase, swe
test_data.freq = test_start_freq;
test_data.gain = dwell_gain_mtr;
test_data.phase = dwell_phase_mtr;
dwell_gain = dwell_gain_mtr;
dwell_phase = dwell_phase_mtr;
}
}
}
@ -1051,15 +1043,13 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
attitude_control->bf_feedforward(orig_bf_feedforward);
// sweep doesn't require gain update so return immediately after setting next test freq
if (input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
// determine next_test_freq for dwell testing
if (sweep_complete){
// if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
if (!is_zero(sweep_tgt.maxgain.freq)) {
next_test_freq = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f;
} else {
next_test_freq = min_sweep_freq;
}
// determine next_test_freq for dwell testing
if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP){
// if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
if (!is_zero(sweep_tgt.maxgain.freq)) {
next_test_freq = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f;
} else {
next_test_freq = min_sweep_freq;
}
return;
}
@ -1081,16 +1071,31 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
// update gains for the max gain tune type
void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
{
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: ph180 freq %f ", sweep_mtr.ph180.freq);
// sweep doesn't require gain update so return immediately after setting next test freq
// determine next_test_freq for dwell testing
if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
// if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
if (!is_zero(sweep_mtr.ph180.freq)) {
next_test_freq = sweep_mtr.ph180.freq - 0.25f * 3.14159f * 2.0f;
reset_maxgains_update_gain_variables();
} else {
next_test_freq = min_sweep_freq;
}
return;
}
switch (test_axis) {
case ROLL:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd);
updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd);
break;
case PITCH:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd);
updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd);
break;
case YAW:
case YAW_D:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd);
updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd);
// rate P and rate D can be non zero for yaw and need to be included in the max allowed gain
if (!is_zero(max_rate_p.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) {
max_rate_p.max_allowed += tune_yaw_rp;
@ -1337,34 +1342,22 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data,
}
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
{
float test_freq_incr = 1.0f * M_PI * 2.0f;
if (!is_equal(start_freq,stop_freq)) {
frq_cnt = 2;
if (!is_zero(sweep_mtr.ph180.freq)) {
freq[frq_cnt] = sweep_mtr.ph180.freq - 0.5f * test_freq_incr;
} else {
freq[frq_cnt] = min_sweep_freq;
}
curr_test.freq = freq[frq_cnt];
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
} else if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (frq_cnt > 2 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 270.0f &&
!find_middle && !found_max_p) {
if (!found_max_p) {
if (test_data.phase > 161.0f && test_data.phase < 270.0f && !find_middle && !found_max_p) {
find_middle = true;
} else if (find_middle && !found_max_p) {
if (phase[frq_cnt] > 161.0f) {
if (test_data.phase > 161.0f) {
// interpolate between frq_cnt-2 and frq_cnt
max_gain_p.freq = linear_interpolate(freq[frq_cnt-2],freq[frq_cnt],161.0f,phase[frq_cnt-2],phase[frq_cnt]);
max_gain_p.gain = linear_interpolate(gain[frq_cnt-2],gain[frq_cnt],161.0f,phase[frq_cnt-2],phase[frq_cnt]);
max_gain_p.freq = linear_interpolate(data_m_two.freq,test_data.freq,161.0f,data_m_two.phase,test_data.phase);
max_gain_p.gain = linear_interpolate(data_m_two.gain,test_data.gain,161.0f,data_m_two.phase,test_data.phase);
} else {
// interpolate between frq_cnt-1 and frq_cnt
max_gain_p.freq = linear_interpolate(freq[frq_cnt],freq[frq_cnt-1],161.0f,phase[frq_cnt],phase[frq_cnt-1]);
max_gain_p.gain = linear_interpolate(gain[frq_cnt],gain[frq_cnt-1],161.0f,phase[frq_cnt],phase[frq_cnt-1]);
max_gain_p.freq = linear_interpolate(test_data.freq,data_m_one.freq,161.0f,test_data.phase,data_m_one.phase);
max_gain_p.gain = linear_interpolate(test_data.gain,data_m_one.gain,161.0f,test_data.phase,data_m_one.phase);
}
max_gain_p.phase = 161.0f;
max_gain_p.max_allowed = powf(10.0f,-1 * (log10f(max_gain_p.gain) * 20.0f + 2.42) / 20.0f);
@ -1372,26 +1365,25 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
max_gain_p.max_allowed = constrain_float(max_gain_p.max_allowed, 0.0f, 2.0f * AUTOTUNE_RP_MAX);
found_max_p = true;
find_middle = false;
data_m_one = {};
data_m_two = {};
if (!is_zero(sweep_mtr.ph270.freq)) {
// set freq cnt back to reinitialize process
frq_cnt = 1; // set to 1 because it will be incremented
// set frequency back at least one test_freq_incr as it will be added
freq[1] = sweep_mtr.ph270.freq - 1.5f * test_freq_incr;
next_freq = sweep_mtr.ph270.freq - 0.5f * test_freq_incr;
}
}
if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f &&
!find_middle && !found_max_d && found_max_p) {
} else if (!found_max_d) {
if (test_data.phase > 251.0f && test_data.phase < 360.0f && !find_middle && !found_max_d && found_max_p) {
find_middle = true;
} else if (find_middle && found_max_p && !found_max_d) {
if (phase[frq_cnt] > 251.0f) {
if (test_data.phase > 251.0f) {
// interpolate between frq_cnt-2 and frq_cnt
max_gain_d.freq = linear_interpolate(freq[frq_cnt-2],freq[frq_cnt],251.0f,phase[frq_cnt-2],phase[frq_cnt]);
max_gain_d.gain = linear_interpolate(gain[frq_cnt-2],gain[frq_cnt],251.0f,phase[frq_cnt-2],phase[frq_cnt]);
max_gain_d.freq = linear_interpolate(data_m_two.freq,test_data.freq,251.0f,data_m_two.phase,test_data.phase);
max_gain_d.gain = linear_interpolate(data_m_two.gain,test_data.gain,251.0f,data_m_two.phase,test_data.phase);
} else {
// interpolate between frq_cnt-1 and frq_cnt
max_gain_d.freq = linear_interpolate(freq[frq_cnt],freq[frq_cnt-1],251.0f,phase[frq_cnt],phase[frq_cnt-1]);
max_gain_d.gain = linear_interpolate(gain[frq_cnt],gain[frq_cnt-1],251.0f,phase[frq_cnt],phase[frq_cnt-1]);
max_gain_d.freq = linear_interpolate(test_data.freq,data_m_one.freq,251.0f,test_data.phase,data_m_one.phase);
max_gain_d.gain = linear_interpolate(test_data.gain,data_m_one.gain,251.0f,test_data.phase,data_m_one.phase);
}
max_gain_d.phase = 251.0f;
max_gain_d.max_allowed = powf(10.0f,-1 * (log10f(max_gain_d.freq * max_gain_d.gain) * 20.0f + 2.42) / 20.0f);
@ -1400,38 +1392,29 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
found_max_d = true;
find_middle = false;
}
// stop progression in frequency.
if ((frq_cnt > 1 && phase[frq_cnt] > 330.0f && !is_zero(phase[frq_cnt])) || found_max_d) {
frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset variables for next test
curr_test.freq = freq[0];
frq_cnt = 0;
start_freq = 0.0f; //initializes next test that uses dwell test
}
if (found_max_d) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset variables for next test
next_freq = 0.0f; //initializes next test that uses dwell test
} else {
if (is_zero(data_m_two.freq) && test_data.phase >= 161.0f && !found_max_p) {
// phase greater than 161 deg won't allow max p to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 161 deg
next_freq = test_data.freq - 0.5f * test_freq_incr;
} else if (is_zero(data_m_two.freq) && test_data.phase >= 251.0f && !found_max_d) {
// phase greater than 251 deg won't allow max d to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 251 deg
next_freq = test_data.freq - 0.5f * test_freq_incr;
} else if (find_middle) {
data_m_one = test_data;
next_freq = test_data.freq - 0.5f * test_freq_incr;
} else {
if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) {
// phase greater than 161 deg won't allow max p to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 161 deg
frq_cnt = 2;
freq[frq_cnt] = freq[frq_cnt] - 0.5f * test_freq_incr;
} else if (frq_cnt == 3 && phase[2] >= 251.0f && !found_max_d) {
// phase greater than 161 deg won't allow max p to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 161 deg
frq_cnt = 2;
freq[frq_cnt] = freq[frq_cnt] - 0.5f * test_freq_incr;
} else if (find_middle) {
freq[frq_cnt] = freq[frq_cnt-1] - 0.5f * test_freq_incr;
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
}
curr_test.freq = freq[frq_cnt];
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
data_m_two = test_data;
next_freq = test_data.freq + test_freq_incr;
}
}
if (found_max_p && found_max_d) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed));
@ -1478,14 +1461,14 @@ void AC_AutoTune_Heli::Log_AutoTune()
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
case YAW_D:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max);
break;
}
}
@ -1603,7 +1586,6 @@ void AC_AutoTune_Heli::reset_vehicle_test_variables()
{
// reset dwell test variables if sweep was interrupted in order to restart sweep
if (!is_equal(start_freq, stop_freq)) {
freq_cnt = 0;
start_freq = 0.0f;
stop_freq = 0.0f;
next_test_freq = 0.0f;
@ -1638,6 +1620,9 @@ void AC_AutoTune_Heli::reset_maxgains_update_gain_variables()
{
max_rate_p = {};
max_rate_d = {};
data_m_one = {};
data_m_two = {};
found_max_p = false;
found_max_d = false;
find_middle = false;

27
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -45,6 +45,13 @@ protected:
//
// methods to load and save gains
//
// sweep_info contains information about a specific test's sweep results
struct sweep_info {
float freq;
float gain;
float phase;
};
// backup original gains and prepare for start of tuning
void backup_gains_and_initialise() override;
@ -144,13 +151,6 @@ private:
float max_allowed;
};
// sweep_info contains information about a specific test's sweep results
struct sweep_info {
float freq;
float gain;
float phase;
};
// FreqRespCalcType is the type of calculation done for the frequency response
enum FreqRespCalcType {
RATE = 0,
@ -179,7 +179,7 @@ private:
void dwell_test_init(float start_frq, float stop_frq, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_run(float &dwell_gain, float &dwell_phase, sweep_info &test_data);
void dwell_test_run(sweep_info &test_data);
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is achieved
@ -195,7 +195,7 @@ private:
void updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq);
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
void updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
// reset the max_gains update gain variables
void reset_maxgains_update_gain_variables();
@ -215,11 +215,6 @@ private:
sweep_info curr_data; // frequency response test results
float next_test_freq; // next test frequency for next test cycle setup
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
uint8_t freq_cnt_max; // counter number for frequency that produced max gain response
// max gain data for rate p tuning
max_gain_data max_rate_p;
// max gain data for rate d tuning
@ -232,6 +227,10 @@ private:
bool found_max_d;
// flag for interpolating to find max response gain
bool find_middle;
// data holding variables for calculations
sweep_info data_m_one;
sweep_info data_m_two;
// updating angle P up variables
// track the maximum phase and freq