Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: tradheli autotune, sqashed commits together

Browse Source
This commit is contained in:
Bill Geyer 2021-01-17 23:16:08 -05:00 committed by Bill Geyer
parent 873924d6cd
commit c757de153d
8 changed files with 1892 additions and 490 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

859
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

File diff suppressed because it is too large Load Diff

53
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -22,6 +22,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AP_Math/AP_Math.h>
#include "AC_AutoTune_FreqResp.h"
#define AUTOTUNE_AXIS_BITMASK_ROLL 1
#define AUTOTUNE_AXIS_BITMASK_PITCH 2
@ -39,8 +40,6 @@
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
#define AUTOTUNE_DWELL_CYCLES 10
class AC_AutoTune
{
public:
@ -175,6 +174,12 @@ protected:
// returns true if rate P gain of zero is acceptable for this vehicle
virtual bool allow_zero_rate_p() = 0;
// returns true if max tested accel is used for parameter
virtual bool set_accel_to_max_test_value() = 0;
// returns true if pilot is allowed to make inputs during test
virtual bool allow_pilot_rp_input() = 0;
// get minimum rate P (for any axis)
virtual float get_rp_min() const = 0;
@ -184,11 +189,15 @@ protected:
// get minimum rate Yaw filter value
virtual float get_yaw_rate_filt_min() const = 0;
// reverse direction for twitch test
virtual bool twitch_reverse_direction() = 0;
// get attitude for slow position hold in autotune mode
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
virtual void Log_AutoTune() = 0;
virtual void Log_AutoTuneDetails() = 0;
virtual void Log_AutoTuneSweep() = 0;
// send message with high level status (e.g. Started, Stopped)
void update_gcs(uint8_t message_id) const;
@ -252,6 +261,8 @@ protected:
TuneType tune_seq[6]; // holds sequence of tune_types to be performed
uint8_t tune_seq_curr; // current tune sequence step
virtual void set_tune_sequence() = 0;
// type of gains to load
enum GainType {
GAIN_ORIGINAL = 0,
@ -343,18 +354,17 @@ protected:
// Feedforward test used to determine Rate FF gain
void rate_ff_test_init();
void rate_ff_test_run(float max_angle_cds, float target_rate_cds);
void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_init(float filt_freq);
void dwell_test_run(uint8_t freq_resp_input, float dwell_freq, float &dwell_gain, float &dwell_phase);
void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// dwell test used to perform frequency dwells for angle gains
void angle_dwell_test_init(float filt_freq);
void angle_dwell_test_run(float dwell_freq, float &dwell_gain, float &dwell_phase);
void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// determines the gain and phase for a dwell
void determine_gain(float tgt_rate, float meas_rate, float freq, float &gain, float &phase, bool &cycles_complete, bool funct_reset);
float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
uint8_t ff_test_phase; // phase of feedforward test
float test_command_filt; // filtered commanded output
@ -370,12 +380,35 @@ protected:
uint8_t freq_cnt;
uint8_t freq_cnt_max;
float curr_test_freq;
bool dwell_complete;
float curr_test_gain;
float curr_test_phase;
Vector3f start_angles;
uint32_t settle_time;
uint32_t phase_out_time;
float waveform_freq_rads; //current frequency for chirp waveform
float start_freq; //start freq for dwell test
float stop_freq; //ending freq for dwell test
float trim_pff_out; // trim output of the PID rate controller for P, I and FF terms
float trim_meas_rate; // trim measured gyro rate
LowPassFilterFloat command_filt; // filtered command
LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second
// sweep_data tracks the overall characteristics in the response to the frequency sweep
struct sweep_data {
float maxgain_freq;
float maxgain_gain;
float maxgain_phase;
float ph180_freq;
float ph180_gain;
float ph180_phase;
float ph270_freq;
float ph270_gain;
float ph270_phase;
uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
};
sweep_data sweep;
struct max_gain_data {
float freq;
float phase;
@ -385,4 +418,8 @@ protected:
max_gain_data max_rate_p;
max_gain_data max_rate_d;
AC_AutoTune_FreqResp freqresp_rate;
AC_AutoTune_FreqResp freqresp_angle;
};

450
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp Normal file
View File

@ -0,0 +1,450 @@
/*
This function receives time history data during a dwell test or frequency sweep and determines the gain and phase of the response to the input.
Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
is set. This function must be reset using the reset flag prior to the next dwell.
*/
#include <AP_HAL/AP_HAL.h>
#include "AC_AutoTune_FreqResp.h"
float AC_AutoTune_FreqResp::update()
{
float dummy = 0.0f;
return dummy;
}
void AC_AutoTune_FreqResp::init(InputType input_type)
{
excitation = input_type;
max_target_cnt = 0;
min_target_cnt = 0;
max_meas_cnt = 0;
min_meas_cnt = 0;
input_start_time_ms = 0;
new_tgt_time_ms = 0;
new_meas_time_ms = 0;
new_target = false;
new_meas = false;
curr_test_freq = 0.0f;
curr_test_gain = 0.0f;
curr_test_phase = 0.0f;
max_accel = 0.0f;
max_meas_rate = 0.0f;
max_command = 0.0f;
meas_peak_info_buffer->clear();
tgt_peak_info_buffer->clear();
cycle_complete = false;
}
// determine_gain - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input.
// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
// is set. This function must be reset using the reset flag prior to the next dwell.
void AC_AutoTune_FreqResp::determine_gain(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 * 6.28 / tgt_freq);
cycle_time_ms = (uint32_t)(1000 * 6.28 / 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 (int 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 / 6.28f;
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 = 6.28f / 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 / 6.28f;
// 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 (tgt_rate > max_target && new_target) {
max_target = tgt_rate;
max_tgt_time = now;
}
if (tgt_rate < min_target && !new_target) {
min_target = tgt_rate;
}
if (meas_rate > max_meas && new_meas) {
max_meas = meas_rate;
max_meas_time = now;
}
if (meas_rate < min_meas && !new_meas) {
min_meas = meas_rate;
}
prev_target = tgt_rate;
prev_meas = meas_rate;
}
// determine_gain_angle - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input.
// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
// is set. This function must be reset using the reset flag prior to the next dwell.
void AC_AutoTune_FreqResp::determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq)
{
uint32_t now = AP_HAL::millis();
float dt = 0.0025;
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 * 6.28 / tgt_freq);
cycle_time_ms = (uint32_t)(1000 * 6.28 / tgt_freq);
}
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;
}
target_rate = (tgt_angle - prev_tgt_angle) / dt;
measured_rate = (meas_angle - prev_meas_angle) / dt;
// 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 (int 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 / 6.28f;
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;
}
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(target_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: 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) {
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) {
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 = 6.28f / 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 / 6.28f;
//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) {
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 (tgt_angle > max_target && new_target) {
max_target = tgt_angle;
max_tgt_time = now;
}
if (tgt_angle < min_target && !new_target) {
min_target = tgt_angle;
}
if (meas_angle > max_meas && new_meas) {
max_meas = meas_angle;
max_meas_time = now;
}
if (meas_angle < min_meas && !new_meas) {
min_meas = meas_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_target = target_rate;
prev_meas = measured_rate;
prev_tgt_angle = tgt_angle;
prev_meas_angle = meas_angle;
}
// push to peak info buffer
void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms)
{
peak_info sample;
sample.curr_count = count;
sample.amplitude = amplitude;
sample.time_ms = time_ms;
meas_peak_info_buffer->push(sample);
}
// pull from peak info buffer
void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
{
peak_info sample;
if (!meas_peak_info_buffer->pop(sample)) {
// no sample
return;
}
count = sample.curr_count;
amplitude = sample.amplitude;
time_ms = sample.time_ms;
}
// push to peak info buffer
void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms)
{
peak_info sample;
sample.curr_count = count;
sample.amplitude = amplitude;
sample.time_ms = time_ms;
tgt_peak_info_buffer->push(sample);
}
// pull from peak info buffer
void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
{
peak_info sample;
if (!tgt_peak_info_buffer->pop(sample)) {
// no sample
return;
}
count = sample.curr_count;
amplitude = sample.amplitude;
time_ms = sample.time_ms;
}

89
libraries/AC_AutoTune/AC_AutoTune_FreqResp.h Normal file
View File

@ -0,0 +1,89 @@
#pragma once
/*
Gain and phase determination algorithm
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#define AUTOTUNE_DWELL_CYCLES 6
class AC_AutoTune_FreqResp {
public:
// Constructor
AC_AutoTune_FreqResp()
{
// initialize test variables
meas_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
tgt_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
}
// CLASS_NO_COPY(AC_PI);
// update calculations
float update();
enum InputType {
DWELL = 0,
SWEEP = 1,
};
void init(InputType input_type);
// determines the gain and phase for a dwell
void determine_gain(float tgt_rate, float meas_rate, float tgt_freq);
// determines the gain and phase for a dwell
void determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq);
// enable external query if cycle is complete and phase and gain data are available
bool is_cycle_complete() { return cycle_complete;}
void reset_cycle_complete() { cycle_complete = false; }
// frequency response accessors
float get_freq() { return curr_test_freq; }
float get_gain() { return curr_test_gain; }
float get_phase() { return curr_test_phase; }
float get_accel_max() { return max_accel; }
private:
float max_target, max_meas, prev_target, prev_meas, prev_tgt_angle, prev_meas_angle;
float min_target, min_meas, temp_meas_ampl, temp_tgt_ampl;
float temp_max_target, temp_min_target, target_rate, measured_rate, max_meas_rate, max_command;
float temp_max_meas, temp_min_meas;
uint32_t temp_max_tgt_time, temp_max_meas_time;
uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms, input_start_time_ms;
uint16_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt;
bool new_target = false;
bool new_meas = false;
bool cycle_complete = false;
float curr_test_freq, curr_test_gain, curr_test_phase;
float max_accel;
InputType excitation;
// sweep_peak_finding_data tracks the peak data
struct sweep_peak_finding_data {
uint16_t count_m1;
float amplitude_m1;
float max_time_m1;
};
sweep_peak_finding_data sweep_meas;
sweep_peak_finding_data sweep_tgt;
//store determine gain data in ring buffer
struct peak_info {
uint16_t curr_count;
float amplitude;
uint32_t time_ms;
};
ObjectBuffer<peak_info> *meas_peak_info_buffer;
ObjectBuffer<peak_info> *tgt_peak_info_buffer;
void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms);
void pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms);
void pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
};

829
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

File diff suppressed because it is too large Load Diff

51
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -24,20 +24,14 @@ class AC_AutoTune_Heli : public AC_AutoTune
{
public:
// constructor
AC_AutoTune_Heli()
{
// tune_seq[0] = 4; // RFF_UP
// tune_seq[1] = 8; // MAX_GAINS
// tune_seq[2] = 0; // RD_UP
// tune_seq[3] = 2; // RP_UP
// tune_seq[2] = 6; // SP_UP
// tune_seq[3] = 9; // tune complete
tune_seq[0] = SP_UP;
tune_seq[1] = TUNE_COMPLETE;
};
AC_AutoTune_Heli();
// save gained, called on disarm
void save_tuning_gains() override;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
void load_test_gains() override;
@ -90,25 +84,52 @@ protected:
// get minimum rate Yaw filter value
float get_yaw_rate_filt_min() const override;
// reverse direction for twitch test
bool twitch_reverse_direction() override { return positive_direction; }
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp);
void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads);
void Log_AutoTuneSweep() override;
void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel);
void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad);
void Log_Write_AutoTuneSweep(float freq, float gain, float phase);
// returns true if rate P gain of zero is acceptable for this vehicle
bool allow_zero_rate_p() override { return true; }
// returns true if max tested accel is used for parameter
bool set_accel_to_max_test_value() override { return false; }
// returns true if pilot is allowed to make inputs during test
bool allow_pilot_rp_input() override
{
if (!use_poshold && tune_type == SP_UP) {
return true;
} else {
return false;
}
}
// send intermittant updates to user on status of tune
void do_gcs_announcements() override;
void set_tune_sequence() override;
AP_Int8 seq_bitmask;
AP_Float min_sweep_freq;
AP_Float max_sweep_freq;
AP_Float max_resp_gain;
private:
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command);
void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_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);
void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d);
void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
void updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
// 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);
uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method
};

28
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -36,15 +36,31 @@
#include "AC_AutoTune_Multi.h"
const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
AP_NESTEDGROUPINFO(AC_AutoTune, 0),
// @Param: AGGR
// @DisplayName: Autotune aggressiveness
// @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
// @Range: 0.05 0.10
// @User: Standard
AP_GROUPINFO("AGGR", 1, AC_AutoTune_Multi, aggressiveness, 0.1f),
// @Param: MIN_D
// @DisplayName: AutoTune minimum D
// @Description: Defines the minimum D gain
// @Range: 0.001 0.006
// @User: Standard
AP_GROUPINFO("MIN_D", 2, AC_AutoTune_Multi, min_d, 0.001f),
AP_GROUPEND
};
// constructor
AC_AutoTune_Multi::AC_AutoTune_Multi()
{
tune_seq[0] = RD_UP;
tune_seq[1] = RD_DOWN;
tune_seq[2] = RP_UP;
tune_seq[3] = SP_DOWN;
tune_seq[4] = SP_UP;
tune_seq[5] = TUNE_COMPLETE;
tune_seq[0] = TUNE_COMPLETE;
AP_Param::setup_object_defaults(this, var_info);
}
void AC_AutoTune_Multi::do_gcs_announcements()

23
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -30,6 +30,9 @@ public:
// save gained, called on disarm
void save_tuning_gains() override;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// load gains
void load_test_gains() override;
@ -77,6 +80,12 @@ protected:
// returns true if rate P gain of zero is acceptable for this vehicle
bool allow_zero_rate_p() override { return false; }
// returns true if pilot is allowed to make inputs during test
bool allow_pilot_rp_input() override { return false; }
// returns true if max tested accel is used for parameter
bool set_accel_to_max_test_value() override { return true; }
// get minimum rate P (for any axis)
float get_rp_min() const override;
@ -86,8 +95,12 @@ protected:
// get minimum yaw rate filter value
float get_yaw_rate_filt_min() const override;
// reverse direction for twitch test
bool twitch_reverse_direction() override { return !positive_direction; }
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
void Log_AutoTuneSweep() override {};
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
@ -111,4 +124,14 @@ private:
// updating_angle_p_up - increase P to ensure the target is reached
// P is increased until we achieve our target within a reasonable time
void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
void set_tune_sequence() override {
tune_seq[0] = RD_UP;
tune_seq[1] = RD_DOWN;
tune_seq[2] = RP_UP;
tune_seq[3] = SP_DOWN;
tune_seq[4] = SP_UP;
tune_seq[5] = TUNE_COMPLETE;
}
};