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

AC_AutoTune: combine update rate and angle update methods in freqresp library

This commit is contained in:
Bill Geyer 2022-01-17 09:37:43 -05:00 committed by Bill Geyer
parent b218f6e5d5
commit 607004ce4c
3 changed files with 81 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

275
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
View File

@ -6,9 +6,10 @@ This library receives time history data (angular rate or angle) during a dwell t
#include "AC_AutoTune_FreqResp.h"
// Initialize the Frequency Response Object. Must be called before running dwell or frequency sweep tests
void AC_AutoTune_FreqResp::init(InputType input_type)
void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type)
{
excitation = input_type;
response = response_type;
max_target_cnt = 0;
min_target_cnt = 0;
max_meas_cnt = 0;
@ -29,188 +30,12 @@ void AC_AutoTune_FreqResp::init(InputType input_type)
cycle_complete = false;
}
// update_rate - this function receives time history data during a dwell and frequency sweep tests for max gain,
// rate P and rate D tuning and determines the gain and phase of the response to the input. For dwell tests once
// the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles
// and the cycle_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and
// cycle_complete flag is set to indicate when to pull the phase and gain data. The flag is reset to enable the next
// cycle to be analyzed.
void AC_AutoTune_FreqResp::update_rate(float tgt_rate, float meas_rate, float tgt_freq)
{
uint32_t now = AP_HAL::millis();
uint32_t half_cycle_time_ms = 0;
uint32_t cycle_time_ms = 0;
if (cycle_complete) {
return;
}
if (!is_zero(tgt_freq)) {
half_cycle_time_ms = (uint32_t)(300 * M_2PI / tgt_freq);
cycle_time_ms = (uint32_t)(1000 * M_2PI / tgt_freq);
}
if (input_start_time_ms == 0) {
input_start_time_ms = now;
}
// cycles are complete! determine gain and phase and exit
if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) {
float delta_time = 0.0f;
float sum_gain = 0.0f;
uint8_t cnt = 0;
uint8_t gcnt = 0;
uint16_t meas_cnt, tgt_cnt;
float meas_ampl = 0.0f;
float tgt_ampl = 0.0f;
uint32_t meas_time = 0;
uint32_t tgt_time = 0;
for (uint8_t i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) {
meas_cnt=0;
tgt_cnt=0;
pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
push_to_meas_buffer(0, 0.0f, 0);
push_to_tgt_buffer(0, 0.0f, 0);
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt));
if (meas_cnt == tgt_cnt && meas_cnt != 0) {
if (tgt_ampl > 0.0f) {
sum_gain += meas_ampl / tgt_ampl;
gcnt++;
}
float d_time = (float)(meas_time - tgt_time);
if (d_time < 2.0f * (float)cycle_time_ms) {
delta_time += d_time;
cnt++;
}
} else if (meas_cnt > tgt_cnt) {
pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
push_to_tgt_buffer(0, 0.0f, 0);
} else if (meas_cnt < tgt_cnt) {
pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
push_to_meas_buffer(0, 0.0f, 0);
}
}
if (gcnt > 0) {
curr_test_gain = sum_gain / gcnt;
}
if (cnt > 0) {
delta_time = delta_time / cnt;
}
curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / M_2PI;
if (curr_test_phase > 360.0f) {
curr_test_phase = curr_test_phase - 360.0f;
}
curr_test_freq = tgt_freq;
cycle_complete = true;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed");
return;
}
// Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought
if (!is_positive(prev_target) && is_positive(tgt_rate) && !new_target && now > new_tgt_time_ms) {
new_target = true;
new_tgt_time_ms = now + half_cycle_time_ms;
// reset max_target
max_target = 0.0f;
max_target_cnt++;
temp_min_target = min_target;
if (min_target_cnt > 0) {
sweep_tgt.max_time_m1 = temp_max_tgt_time;
temp_max_tgt_time = max_tgt_time;
sweep_tgt.count_m1 = min_target_cnt - 1;
sweep_tgt.amplitude_m1 = temp_tgt_ampl;
temp_tgt_ampl = temp_max_target - temp_min_target;
push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time);
}
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
} else if (is_positive(prev_target) && !is_positive(tgt_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) {
new_target = false;
new_tgt_time_ms = now + half_cycle_time_ms;
min_target_cnt++;
temp_max_target = max_target;
min_target = 0.0f;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
}
// Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought
if (!is_positive(prev_meas) && is_positive(meas_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) {
new_meas = true;
new_meas_time_ms = now + half_cycle_time_ms;
// reset max_meas
max_meas = 0.0f;
max_meas_cnt++;
temp_min_meas = min_meas;
if (min_meas_cnt > 0 && min_target_cnt > 0) {
sweep_meas.max_time_m1 = temp_max_meas_time;
temp_max_meas_time = max_meas_time;
sweep_meas.count_m1 = min_meas_cnt - 1;
sweep_meas.amplitude_m1 = temp_meas_ampl;
temp_meas_ampl = temp_max_meas - temp_min_meas;
push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time);
if (excitation == SWEEP) {
float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1);
if (!is_zero(tgt_period)) {
curr_test_freq = M_2PI / tgt_period;
} else {
curr_test_freq = 0.0f;
}
if (!is_zero(sweep_tgt.amplitude_m1)) {
curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1;
} else {
curr_test_gain = 0.0f;
}
curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / M_2PI;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f sweepgain=%f", (double)(curr_test_freq), (double)(curr_test_gain));
cycle_complete = true;
}
}
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
} else if (is_positive(prev_meas) && !is_positive(meas_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) {
new_meas = false;
new_meas_time_ms = now + half_cycle_time_ms;
min_meas_cnt++;
temp_max_meas = max_meas;
min_meas = 0.0f;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
}
if (new_target) {
if (tgt_rate > max_target) {
max_target = tgt_rate;
max_tgt_time = now;
}
} else {
if (tgt_rate < min_target) {
min_target = tgt_rate;
}
}
if (new_meas) {
if (meas_rate > max_meas) {
max_meas = meas_rate;
max_meas_time = now;
}
} else {
if (meas_rate < min_meas) {
min_meas = meas_rate;
}
}
prev_target = tgt_rate;
prev_meas = meas_rate;
}
// update_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning
// and determines the gain, phase, and max acceleration of the response to the input. For dwell tests once the designated number
// of cycles are complete, the average of the gain, phase, and max acceleration are determined over the last 5 cycles and the
// cycles_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and cycle_complete flag is set
// to indicate when to pull the phase and gain data. The flag is reset to enable the next cycle to be analyzed.
void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq)
void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp, float tgt_freq)
{
uint32_t now = AP_HAL::millis();
@ -229,14 +54,21 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
if (input_start_time_ms == 0) {
input_start_time_ms = now;
prev_tgt_angle = tgt_angle;
prev_meas_angle = meas_angle;
prev_target = 0.0f;
prev_meas = 0.0f;
if (response == ANGLE) {
prev_tgt_resp = tgt_resp;
prev_meas_resp = meas_resp;
prev_target = 0.0f;
prev_meas = 0.0f;
}
}
target_rate = (tgt_angle - prev_tgt_angle) / dt;
measured_rate = (meas_angle - prev_meas_angle) / dt;
if (response == ANGLE) {
target_rate = (tgt_resp - prev_tgt_resp) / dt;
measured_rate = (meas_resp - prev_meas_resp) / dt;
} else {
target_rate = tgt_resp;
measured_rate = meas_resp;
}
// cycles are complete! determine gain and phase and exit
if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) {
@ -287,14 +119,20 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
if (curr_test_phase > 360.0f) {
curr_test_phase = curr_test_phase - 360.0f;
}
float dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f;
if (!is_zero(max_command)) {
// normalize accel for input size
dwell_max_accel = dwell_max_accel / (2.0f * max_command);
}
if (dwell_max_accel > max_accel) {
max_accel = dwell_max_accel;
// determine max accel for angle response type test
float dwell_max_accel;
if (response == ANGLE) {
dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f;
if (!is_zero(max_command)) {
// normalize accel for input size
dwell_max_accel = dwell_max_accel / (2.0f * max_command);
}
if (dwell_max_accel > max_accel) {
max_accel = dwell_max_accel;
}
}
curr_test_freq = tgt_freq;
cycle_complete = true;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed");
@ -302,7 +140,9 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
}
// Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought
if (is_positive(prev_target) && !is_positive(target_rate) && !new_target && now > new_tgt_time_ms) {
if (((response == ANGLE && is_positive(prev_target) && !is_positive(target_rate))
|| (response == RATE && !is_positive(prev_target) && is_positive(target_rate)))
&& !new_target && now > new_tgt_time_ms) {
new_target = true;
new_tgt_time_ms = now + half_cycle_time_ms;
// reset max_target
@ -317,19 +157,21 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
temp_tgt_ampl = temp_max_target - temp_min_target;
push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time);
}
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_tgt_cnt=%f", (double)(max_target_cnt));
} else if (!is_positive(prev_target) && is_positive(target_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) {
} else if (((response == ANGLE && !is_positive(prev_target) && is_positive(target_rate))
|| (response == RATE && is_positive(prev_target) && !is_positive(target_rate)))
&& new_target && now > new_tgt_time_ms && max_target_cnt > 0) {
new_target = false;
new_tgt_time_ms = now + half_cycle_time_ms;
min_target_cnt++;
temp_max_target = max_target;
min_target = 0.0f;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
}
// Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought
if (is_positive(prev_meas) && !is_positive(measured_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) {
if (((response == ANGLE && is_positive(prev_meas) && !is_positive(measured_rate))
|| (response == RATE && !is_positive(prev_meas) && is_positive(measured_rate)))
&& !new_meas && now > new_meas_time_ms && max_target_cnt > 0) {
new_meas = true;
new_meas_time_ms = now + half_cycle_time_ms;
// reset max_meas
@ -357,55 +199,56 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
curr_test_gain = 0.0f;
}
curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / M_2PI;
//gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
cycle_complete = true;
}
}
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_meas_cnt=%f", (double)(max_meas_cnt));
} else if (!is_positive(prev_meas) && is_positive(measured_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) {
} else if (((response == ANGLE && !is_positive(prev_meas) && is_positive(measured_rate))
|| (response == RATE && is_positive(prev_meas) && !is_positive(measured_rate)))
&& new_meas && now > new_meas_time_ms && max_meas_cnt > 0) {
new_meas = false;
new_meas_time_ms = now + half_cycle_time_ms;
min_meas_cnt++;
temp_max_meas = max_meas;
min_meas = 0.0f;
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
}
if (new_target) {
if (tgt_angle > max_target) {
max_target = tgt_angle;
if (tgt_resp > max_target) {
max_target = tgt_resp;
max_tgt_time = now;
}
} else {
if (tgt_angle < min_target) {
min_target = tgt_angle;
if (tgt_resp < min_target) {
min_target = tgt_resp;
}
}
if (new_meas) {
if (meas_angle > max_meas) {
max_meas = meas_angle;
if (meas_resp > max_meas) {
max_meas = meas_resp;
max_meas_time = now;
}
} else {
if (meas_angle < min_meas) {
min_meas = meas_angle;
if (meas_resp < min_meas) {
min_meas = meas_resp;
}
}
if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) {
if (measured_rate > max_meas_rate) {
max_meas_rate = measured_rate;
}
if (command > max_command) {
max_command = command;
if (response == ANGLE) {
if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) {
if (measured_rate > max_meas_rate) {
max_meas_rate = measured_rate;
}
if (command > max_command) {
max_command = command;
}
}
prev_tgt_resp = tgt_resp;
prev_meas_resp = meas_resp;
}
prev_target = target_rate;
prev_meas = measured_rate;
prev_tgt_angle = tgt_angle;
prev_meas_angle = meas_angle;
}
// push measured peak info to buffer

24
libraries/AC_AutoTune/AC_AutoTune_FreqResp.h
View File

@ -22,15 +22,18 @@ public:
SWEEP = 1,
};
// Enumeration of type
enum ResponseType {
RATE = 0,
ANGLE = 1,
};
// Initialize the Frequency Response Object.
// Must be called before running dwell or frequency sweep tests
void init(InputType input_type);
// Determines the gain and phase based on rate response for a dwell or sweep
void update_rate(float tgt_rate, float meas_rate, float tgt_freq);
void init(InputType input_type, ResponseType response_type);
// Determines the gain and phase based on angle response for a dwell or sweep
void update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq);
void update(float command, float tgt_resp, float meas_resp, float tgt_freq);
// Enable external query if cycle is complete and freq response data are available
bool is_cycle_complete() { return cycle_complete;}
@ -78,8 +81,8 @@ private:
// maximum target value from previous cycle
float prev_target;
// maximum target angle from previous cycle
float prev_tgt_angle;
// maximum target response from previous cycle
float prev_tgt_resp;
// holds target amplitude for gain calculation
float temp_tgt_ampl;
@ -117,8 +120,8 @@ private:
// maximum measured value from previous cycle
float prev_meas;
// maximum measured angle from previous cycle
float prev_meas_angle;
// maximum measured response from previous cycle
float prev_meas_resp;
// holds measured amplitude for gain calculation
float temp_meas_ampl;
@ -152,6 +155,9 @@ private:
// Input type for frequency response object
InputType excitation;
// Response type for frequency response object
ResponseType response;
// sweep_peak_finding_data tracks the peak data
struct sweep_peak_finding_data {
uint16_t count_m1;

14
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -155,11 +155,11 @@ void AC_AutoTune_Heli::test_init()
}
if (!is_equal(start_freq,stop_freq)) {
// initialize determine_gain function whenever test is initialized
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP);
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE);
dwell_test_init(start_freq, stop_freq);
} else {
// initialize determine_gain function whenever test is initialized
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL);
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE);
dwell_test_init(start_freq, start_freq);
}
if (!is_zero(start_freq)) {
@ -185,11 +185,11 @@ void AC_AutoTune_Heli::test_init()
if (!is_equal(start_freq,stop_freq)) {
// initialize determine gain function
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP);
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE);
angle_dwell_test_init(start_freq, stop_freq);
} else {
// initialize determine gain function
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL);
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE);
angle_dwell_test_init(start_freq, start_freq);
}
@ -1082,9 +1082,9 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
// 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_rate(filt_target_rate,rotation_rate, dwell_freq);
freqresp_rate.update(command_out,filt_target_rate,rotation_rate, dwell_freq);
} else {
freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq);
freqresp_rate.update(command_out,command_out,rotation_rate, dwell_freq);
}
if (freqresp_rate.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
@ -1282,7 +1282,7 @@ 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
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
freqresp_angle.update_angle(command_out, filt_target_rate, rotation_rate, dwell_freq);
freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq);
if (freqresp_angle.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_angle.get_freq();