zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

gps-1.3.1
Bill Geyer 3 years ago committed by Bill Geyer
parent
b218f6e5d5
commit
607004ce4c
3 changed files with 81 additions and 232 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 275
      libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
  2. 24
      libraries/AC_AutoTune/AC_AutoTune_FreqResp.h
  3. 14
      libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

275
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
Unescape View File

@ -6,9 +6,10 @@ This library receives time history data (angular rate or angle) during a dwell t @@ -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) @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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
Unescape View File

@ -22,15 +22,18 @@ public: @@ -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: @@ -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: @@ -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: @@ -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
Unescape View File

@ -155,11 +155,11 @@ void AC_AutoTune_Heli::test_init() @@ -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() @@ -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, @@ -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 @@ -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();

Write Preview
Loading…
Cancel
Save