Skip to content

Commit

Permalink
AC_AutoTune: reworked dwell_test and updating ff up
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Apr 12, 2024
1 parent 7eaacda commit fc8cf52
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 64 deletions.
69 changes: 31 additions & 38 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,11 +136,12 @@ void AC_AutoTune_Heli::test_init()
FreqRespInput freq_resp_input = TARGET;
switch (tune_type) {
case RFF_UP:
freq_cnt = 12;
test_freq[freq_cnt] = 0.25f * 3.14159f * 2.0f;
curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
if (!is_positive(next_test_freq)) {
start_freq = 0.25f * 3.14159f * 2.0f;
} else {
start_freq = next_test_freq;
}
stop_freq = start_freq;

attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
Expand Down Expand Up @@ -295,7 +296,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]);
dwell_test_run(test_gain[freq_cnt], test_phase[freq_cnt], curr_data);

}

Expand Down Expand Up @@ -374,8 +375,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
case MAX_GAINS:
if (is_equal(start_freq,stop_freq)) {
// announce results of dwell
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(test_phase[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase));
if (tune_type == RP_UP) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp));
} else if (tune_type == RD_UP) {
Expand Down Expand Up @@ -755,7 +756,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)
void AC_AutoTune_Heli::dwell_test_run(float &dwell_gain, float &dwell_phase, sweep_info &test_data)
{
float gyro_reading = 0.0f;
float command_reading = 0.0f;
Expand Down Expand Up @@ -917,13 +918,19 @@ void AC_AutoTune_Heli::dwell_test_run(float &dwell_gain, float &dwell_phase)
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
curr_test = curr_test_tgt;
} else {
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) {
curr_test = curr_test_mtr;
} else {
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 @@ -1019,14 +1026,14 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_ff_up(tune_roll_rff, test_freq, test_gain, test_phase, freq_cnt);
updating_rate_ff_up(tune_roll_rff, curr_data, next_test_freq);
break;
case PITCH:
updating_rate_ff_up(tune_pitch_rff, test_freq, test_gain, test_phase, freq_cnt);
updating_rate_ff_up(tune_pitch_rff, curr_data, next_test_freq);
break;
case YAW:
case YAW_D:
updating_rate_ff_up(tune_yaw_rff, test_freq, test_gain, test_phase, freq_cnt);
updating_rate_ff_up(tune_yaw_rff, curr_data, next_test_freq);
// TODO make FF updating routine determine when to set rff gain to zero based on A/C response
if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && counter == AUTOTUNE_SUCCESS_COUNT) {
tune_yaw_rff = 0.0f;
Expand Down Expand Up @@ -1133,38 +1140,28 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)

// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is achieved
void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float *freq, float *gain, float *phase, uint8_t &frq_cnt)
void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq)
{
float test_freq_incr = 0.05f * 3.14159f * 2.0f;

if (phase[frq_cnt] > 15.0f) {
curr_test.freq = curr_test.freq - test_freq_incr;
freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] < 0.0f) {
curr_test.freq = curr_test.freq + test_freq_incr;
freq[frq_cnt] = curr_test.freq;
next_freq = test_data.freq;
if (test_data.phase > 15.0f) {
next_freq -= test_freq_incr;
} else if (test_data.phase < 0.0f) {
next_freq += test_freq_incr;
} else {
if ((gain[frq_cnt] > 0.1 && gain[frq_cnt] < 0.93) || gain[frq_cnt] > 0.98) {
if ((test_data.gain > 0.1 && test_data.gain < 0.93) || test_data.gain > 0.98) {
if (tune_ff > 0.0f) {
tune_ff = 0.95f * tune_ff / gain[frq_cnt];
tune_ff = 0.95f * tune_ff / test_data.gain;
} else {
tune_ff = 0.03f;
}
} else if (gain[frq_cnt] >= 0.93 && gain[frq_cnt] <= 0.98) {
} else if (test_data.gain >= 0.93 && test_data.gain <= 0.98) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test.freq and frq_cnt for next test
curr_test.freq = freq[0];
frq_cnt = 0;
// reset next_freq for next test
next_freq = 0.0f;
tune_ff = constrain_float(tune_ff,0.0f,1.0f);
}
}

if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test
} else {
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
}
}

// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
Expand Down Expand Up @@ -1616,11 +1613,7 @@ void AC_AutoTune_Heli::reset_vehicle_test_variables()
// reset the update gain variables for heli
void AC_AutoTune_Heli::reset_update_gain_variables()
{
// reset feedforward update gain variables
ff_up_first_iter = true;
first_dir_complete = false;

// reset max gain variables
// reset max gain variables
reset_maxgains_update_gain_variables();

// reset rd_up variables
Expand Down
47 changes: 21 additions & 26 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,10 +144,12 @@ class AC_AutoTune_Heli : public AC_AutoTune
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;
// 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 {
Expand All @@ -165,11 +167,11 @@ 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);
void dwell_test_run(float &dwell_gain, float &dwell_phase, sweep_info &test_data);

// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is achieved
void updating_rate_ff_up(float &tune_ff, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
void updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq);

// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p);
Expand Down Expand Up @@ -198,11 +200,18 @@ class AC_AutoTune_Heli : public AC_AutoTune
// define input type as Dwell or Sweep. Used through entire class
AC_AutoTune_FreqResp::InputType input_type;

// updating rate FF variables
// flag for completion of the initial direction for the feedforward test
bool first_dir_complete;
// feedforward gain resulting from testing in the initial direction
float first_dir_rff;
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
max_gain_data max_rate_d;

// updating max gain variables
// flag for finding maximum p gain
Expand Down Expand Up @@ -237,27 +246,14 @@ class AC_AutoTune_Heli : public AC_AutoTune
uint8_t num_dwell_cycles;
float test_start_freq;

// number of cycles to complete before running frequency response calculations
float pre_calc_cycles;

float pre_calc_cycles; // number of cycles to complete before running frequency response calculations
float command_out; // test axis command output
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

// sweep_info contains information about a specific test's sweep results
struct sweep_info {
float freq;
float gain;
float phase;
};
sweep_info curr_test;
sweep_info curr_test_mtr;
sweep_info curr_test_tgt;
sweep_info test[20];

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
Expand Down Expand Up @@ -287,7 +283,6 @@ class AC_AutoTune_Heli : public AC_AutoTune
sweep_info maxgain;
sweep_info ph180;
sweep_info ph270;

uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
};
sweep_data sweep_mtr;
Expand Down

0 comments on commit fc8cf52

Please sign in to comment.