Skip to content

Commit

Permalink
AC_AutoTune: reworked updating max gains
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Apr 18, 2024
1 parent 459ec58 commit 2b0c83a
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 111 deletions.
179 changes: 82 additions & 97 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,21 +154,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);

Expand Down Expand Up @@ -291,7 +287,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);

}

Expand Down Expand Up @@ -399,7 +395,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;

Expand Down Expand Up @@ -751,7 +747,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;
Expand Down Expand Up @@ -916,8 +912,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) {
Expand All @@ -926,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_mtr;
test_data.phase = dwell_phase_mtr;
dwell_gain = dwell_gain_mtr;
dwell_phase = dwell_phase_mtr;
}
}
}
Expand Down Expand Up @@ -1043,15 +1035,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;
}
Expand All @@ -1073,16 +1063,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;
Expand Down Expand Up @@ -1329,61 +1334,48 @@ 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);
// limit max gain allowed to be no more than 2x the max p gain limit to keep initial gains bounded
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);
Expand All @@ -1392,38 +1384,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));
Expand All @@ -1440,14 +1423,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;
}
}
Expand Down Expand Up @@ -1565,7 +1548,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;
Expand Down Expand Up @@ -1600,6 +1582,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;
Expand Down
27 changes: 13 additions & 14 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,13 @@ class AC_AutoTune_Heli : public AC_AutoTune
//
// 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;
Expand Down Expand Up @@ -144,13 +151,6 @@ class AC_AutoTune_Heli : public AC_AutoTune
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,
Expand All @@ -167,7 +167,7 @@ class AC_AutoTune_Heli : public AC_AutoTune
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
Expand All @@ -183,7 +183,7 @@ class AC_AutoTune_Heli : public AC_AutoTune
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();
Expand All @@ -203,11 +203,6 @@ class AC_AutoTune_Heli : public AC_AutoTune
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
Expand All @@ -220,6 +215,10 @@ class AC_AutoTune_Heli : public AC_AutoTune
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
Expand Down

0 comments on commit 2b0c83a

Please sign in to comment.