mirror of https://github.com/ArduPilot/ardupilot
AC_AutoTune: more suggested changes
This commit is contained in:
parent
088b7ec094
commit
f50bb54112
|
@ -262,14 +262,16 @@ void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude,
|
||||||
sample.curr_count = count;
|
sample.curr_count = count;
|
||||||
sample.amplitude = amplitude;
|
sample.amplitude = amplitude;
|
||||||
sample.time_ms = time_ms;
|
sample.time_ms = time_ms;
|
||||||
|
if (meas_peak_info_buffer != nullptr) {
|
||||||
meas_peak_info_buffer->push(sample);
|
meas_peak_info_buffer->push(sample);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// pull measured peak info from buffer
|
// pull measured peak info from buffer
|
||||||
void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms)
|
void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms)
|
||||||
{
|
{
|
||||||
peak_info sample;
|
peak_info sample;
|
||||||
if (!meas_peak_info_buffer->pop(sample)) {
|
if ((meas_peak_info_buffer == nullptr) || !meas_peak_info_buffer->pop(sample)) {
|
||||||
// no sample
|
// no sample
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -285,15 +287,16 @@ void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, u
|
||||||
sample.curr_count = count;
|
sample.curr_count = count;
|
||||||
sample.amplitude = amplitude;
|
sample.amplitude = amplitude;
|
||||||
sample.time_ms = time_ms;
|
sample.time_ms = time_ms;
|
||||||
|
if (tgt_peak_info_buffer != nullptr) {
|
||||||
tgt_peak_info_buffer->push(sample);
|
tgt_peak_info_buffer->push(sample);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// pull target peak info from buffer
|
// pull target peak info from buffer
|
||||||
void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms)
|
void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms)
|
||||||
{
|
{
|
||||||
peak_info sample;
|
peak_info sample;
|
||||||
if (!tgt_peak_info_buffer->pop(sample)) {
|
if ((tgt_peak_info_buffer == nullptr) || !tgt_peak_info_buffer->pop(sample)) {
|
||||||
// no sample
|
// no sample
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1241,7 +1241,9 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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
|
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not
|
||||||
|
// exceed max response gain. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
|
||||||
|
// max gain to 6db gain margin is determined for a unity feedback controller.
|
||||||
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p)
|
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p)
|
||||||
{
|
{
|
||||||
float test_freq_incr = 0.25f * M_2PI;
|
float test_freq_incr = 0.25f * M_2PI;
|
||||||
|
@ -1265,7 +1267,9 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum
|
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response
|
||||||
|
// gain is at a minimum. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
|
||||||
|
// max gain to 6db gain margin is determined for a unity feedback controller.
|
||||||
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d)
|
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d)
|
||||||
{
|
{
|
||||||
float test_freq_incr = 0.25f * M_2PI; // set for 1/4 hz increments
|
float test_freq_incr = 0.25f * M_2PI; // set for 1/4 hz increments
|
||||||
|
@ -1291,7 +1295,8 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// updating_angle_p_up - determines maximum angle p gain for pitch and roll
|
// updating_angle_p_up - determines maximum angle p gain for pitch and roll. This is accomplished by determining the frequency
|
||||||
|
// for the maximum response gain that is the disturbance rejection peak.
|
||||||
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq)
|
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq)
|
||||||
{
|
{
|
||||||
float test_freq_incr = 0.5f * M_2PI;
|
float test_freq_incr = 0.5f * M_2PI;
|
||||||
|
@ -1382,7 +1387,11 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
|
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur. This uses the frequency
|
||||||
|
// response of motor class input to rate response to determine the max allowable gain for rate P gain. A phase of 161 deg is used to
|
||||||
|
// determine analytically the max gain to 6db gain margin for a unity feedback controller. Since acceleration can be more noisy, the
|
||||||
|
// response of the motor class input to rate response to determine the max allowable gain for rate D gain. A phase of 251 deg is used
|
||||||
|
// to determine analytically the max gain to 6db gain margin for a unity feedback controller.
|
||||||
void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
|
void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
|
||||||
{
|
{
|
||||||
float test_freq_incr = 0.5f * M_2PI;
|
float test_freq_incr = 0.5f * M_2PI;
|
||||||
|
|
Loading…
Reference in New Issue