Skip to content

Commit

Permalink
AC_Autotune: remove code from no longer used RFF limited rate method
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 11, 2024
1 parent f712299 commit dadb634
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 103 deletions.
88 changes: 7 additions & 81 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,6 @@ const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {
AC_AutoTune_Heli::AC_AutoTune_Heli()
{
tune_seq[0] = TUNE_COMPLETE;
rff_info_buffer = new ObjectBuffer<rff_info>(80);
AP_Param::setup_object_defaults(this, var_info);
}

Expand Down Expand Up @@ -153,7 +152,7 @@ void AC_AutoTune_Heli::test_init()
// freq_resp_amplitude = 10.0f; // increase amplitude with limited rate to get desired square wave rate response
// variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
calc_type = RFF;
calc_type = RATE;
freq_resp_input = TARGET;
pre_calc_cycles = 1.0f;
num_dwell_cycles = 3;
Expand Down Expand Up @@ -785,18 +784,6 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am
cycle_complete_mtr = false;
sweep_complete = false;

rff_full_cycle = false;
rff_input_sum = 0.0f;
rff_response_sum = 0.0f;
rff_on_rate_limit = false;
rff_gain = 0.0f;
if (rff_info_buffer != nullptr) {
rff_info_buffer->clear();
}
input_sum = 0.0f;
response_sum = 0.0f;


}

void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
Expand Down Expand Up @@ -849,7 +836,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
if (test_calc_type == DRB) {
tgt_rate_reading = (target_angle_cd) / 5730.0f;
gyro_reading = ((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd) / 5730.0f;
} else if (test_calc_type == RATE || test_calc_type == RFF) {
} else if (test_calc_type == RATE) {
tgt_rate_reading = attitude_control->rate_bf_targets().x;
gyro_reading = ahrs_view->get_gyro().x;
} else {
Expand All @@ -863,7 +850,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
if (test_calc_type == DRB) {
tgt_rate_reading = (target_angle_cd) / 5730.0f;
gyro_reading = ((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd) / 5730.0f;
} else if (test_calc_type == RATE || test_calc_type == RFF) {
} else if (test_calc_type == RATE) {
tgt_rate_reading = attitude_control->rate_bf_targets().y;
gyro_reading = ahrs_view->get_gyro().y;
} else {
Expand All @@ -878,7 +865,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
if (test_calc_type == DRB) {
tgt_rate_reading = (target_angle_cd) / 5730.0f;
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading - target_angle_cd)) / 5730.0f;
} else if (test_calc_type == RATE || test_calc_type == RFF) {
} else if (test_calc_type == RATE) {
tgt_rate_reading = attitude_control->rate_bf_targets().z;
gyro_reading = ahrs_view->get_gyro().z;
} else {
Expand Down Expand Up @@ -906,10 +893,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
command_out = command_filt.apply((command_reading - filt_command_reading.get()),
AP::scheduler().get_loop_period_s());

float rff_input_avg;
float rff_response_avg;
push_to_rff_info_buffer(tgt_rate_reading, gyro_reading, rff_input_avg, rff_response_avg);

float dwell_gain_mtr = 0.0f;
float dwell_phase_mtr = 0.0f;
float dwell_gain_tgt = 0.0f;
Expand All @@ -919,28 +902,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq);
freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq);

if (test_input_type == AC_AutoTune_FreqResp::InputType::DWELL && test_calc_type == RFF) {
float rate_limit = radians(4.90);
if (fabsf(tgt_rate_reading) > rate_limit) {
rff_on_rate_limit = true;
} else if (rff_on_rate_limit && fabsf(tgt_rate_reading) < rate_limit) {
rff_input_sum += fabsf(rff_input_avg);
rff_response_sum += fabsf(rff_response_avg);
if (rff_full_cycle) {
// only calculate gain for full cycle
if (fabsf(rff_input_sum) > 0.0f) {
rff_gain = rff_response_sum / rff_input_sum;
}
rff_full_cycle = false;
} else {
rff_full_cycle = true;
}
rff_on_rate_limit = false;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: output= %f, input=%f, RFF Gain=%f", (double)(rff_response_avg),(double)(rff_input_avg),(double)(rff_gain));

}
}

if (freqresp_mtr.is_cycle_complete()) {
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
if (is_zero(curr_test_mtr.freq) && freqresp_mtr.get_freq() < test_start_freq) {
Expand Down Expand Up @@ -991,17 +952,9 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
curr_test = curr_test_tgt;
} else {
if (test_calc_type == RFF) {
test_data.freq = test_start_freq;
// test_data.gain = rff_gain;
test_data.gain = dwell_gain_tgt;
// test_data.phase = 10.0f;
test_data.phase = dwell_phase_tgt;
} else {
test_data.freq = test_start_freq;
test_data.gain = dwell_gain_tgt;
test_data.phase = dwell_phase_tgt;
}
test_data.freq = test_start_freq;
test_data.gain = dwell_gain_tgt;
test_data.phase = dwell_phase_tgt;
}
} else {
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
Expand Down Expand Up @@ -1746,31 +1699,4 @@ bool AC_AutoTune_Heli::exceeded_freq_range(float frequency)
return ret;
}

// push rff_info to buffer
void AC_AutoTune_Heli::push_to_rff_info_buffer(float input, float response, float &input_avg, float &response_avg)
{
rff_info sample;
float num_samples = 80.0f;
//if buffer isn't full, fill with zeros
while (rff_info_buffer->space() != 0) {
sample.input = 0.0f;
sample.response = 0.0f;
rff_info_buffer->push(sample);
}

if (!rff_info_buffer->pop(sample)) {
// no sample
return;
}
// remove popped data and add pushed data to the summation
input_sum = input_sum - sample.input + input;
response_sum = response_sum - sample.response + response;
input_avg = input_sum / num_samples;
response_avg = response_sum / num_samples;
// push new data
sample.input = input;
sample.response = response;
rff_info_buffer->push(sample);
}

#endif // AC_AUTOTUNE_ENABLED
22 changes: 0 additions & 22 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,6 @@ class AC_AutoTune_Heli : public AC_AutoTune
RATE = 0,
ANGLE = 1,
DRB = 2,
RFF = 3,
};

enum FreqRespInput {
Expand Down Expand Up @@ -303,27 +302,6 @@ class AC_AutoTune_Heli : public AC_AutoTune
bool cycle_complete_tgt;
bool cycle_complete_mtr;

//store response data in ring buffer
struct rff_info {
float input;
float response;
};
float input_sum;
float response_sum;

// rff gain calc for dwell
float rff_input_sum;
float rff_response_sum;
float rff_gain;
bool rff_on_rate_limit;
bool rff_full_cycle;

// Buffer object for measured peak data
ObjectBuffer<rff_info> *rff_info_buffer;

// Push data into rff_info buffer object
void push_to_rff_info_buffer(float input, float response, float &input_avg, float &response_avg);

Chirp chirp_input;
};

Expand Down

0 comments on commit dadb634

Please sign in to comment.