aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AutoTune: switch remaining variables over to using filter library

Browse Source
This commit is contained in:
Bill Geyer 2022-01-01 22:33:52 -05:00 committed by Bill Geyer
parent 024645f0be
commit 9945c80fb4
2 changed files with 71 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -146,11 +146,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);
dwell_test_init(stop_freq);
dwell_test_init(start_freq, stop_freq);
} else {
// initialize determine_gain function whenever test is initialized
freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL);
dwell_test_init(start_freq);
dwell_test_init(start_freq, start_freq);
}
if (!is_zero(start_freq)) {
// 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it.
@ -169,11 +169,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);
angle_dwell_test_init(stop_freq);
angle_dwell_test_init(start_freq, stop_freq);
} else {
// initialize determine gain function
freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL);
angle_dwell_test_init(start_freq);
angle_dwell_test_init(start_freq, start_freq);
}
// TODO add time limit for sweep test
@ -809,7 +809,7 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
}
void AC_AutoTune_Heli::dwell_test_init(float filt_freq)
void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq)
{
rotation_rate_filt.reset(0);
rotation_rate_filt.set_cutoff_frequency(filt_freq);
@ -835,6 +835,13 @@ void AC_AutoTune_Heli::dwell_test_init(float filt_freq)
curr_test_phase = 0.0f;
}
// filter at lower frequency to remove steady state
filt_command_reading.set_cutoff_frequency(0.2f * start_frq);
filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq);
filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq);
filt_attitude_cd.set_cutoff_frequency(0.2f * start_frq);
filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq);
// save the trim output from PID controller
float ff_term = 0.0f;
float p_term = 0.0f;
@ -876,7 +883,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq;
}
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
@ -901,26 +907,24 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
dwell_freq = waveform_freq_rads;
}
}
filt_attitude_cd.x += alpha * (attitude_cd.x - filt_attitude_cd.x);
filt_attitude_cd.y += alpha * (attitude_cd.y - filt_attitude_cd.y);
filt_attitude_cd.z += alpha * wrap_180_cd(attitude_cd.z - filt_attitude_cd.z);
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
filt_attitude_cd.apply(attitude_cd, AP::scheduler().get_loop_period_s());
Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x);
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
} else {
target_rate_cds = 0.0f;
settle_time--;
dwell_start_time_ms = now;
trim_command = command_out;
filt_attitude_cd = attitude_cd;
trim_attitude_cd = attitude_cd;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
filt_attitude_cd.reset(attitude_cd);
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
}
// limit rate correction for position hold
Vector3f trim_rate_cds;
trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x);
trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y);
trim_rate_cds.z = att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.z);
trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_attitude_cd.get().x);
trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_attitude_cd.get().y);
trim_rate_cds.z = att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.get().z);
trim_rate_cds.x = constrain_float(trim_rate_cds.x, -15000.0f, 15000.0f);
trim_rate_cds.y = constrain_float(trim_rate_cds.y, -15000.0f, 15000.0f);
trim_rate_cds.z = constrain_float(trim_rate_cds.z, -15000.0f, 15000.0f);
@ -989,21 +993,21 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s());
filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s());
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
filt_command_reading.reset(command_reading);
filt_gyro_reading.reset(gyro_reading);
filt_tgt_rate_reading.reset(tgt_rate_reading);
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
command_out = command_filt.apply((command_reading - filt_command_reading.get()),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase or just start if sweep
@ -1069,7 +1073,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
}
}
void AC_AutoTune_Heli::angle_dwell_test_init(float filt_freq)
void AC_AutoTune_Heli::angle_dwell_test_init(float start_frq, float filt_freq)
{
rotation_rate_filt.set_cutoff_frequency(filt_freq);
command_filt.set_cutoff_frequency(filt_freq);
@ -1103,6 +1107,14 @@ void AC_AutoTune_Heli::angle_dwell_test_init(float filt_freq)
filt_target_rate = 0.0f;
break;
}
// filter at lower frequency to remove steady state
filt_command_reading.set_cutoff_frequency(0.2f * start_frq);
filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq);
filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq);
filt_attitude_cd.set_cutoff_frequency(0.2f * start_frq);
filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq);
if (!is_equal(start_freq, stop_freq)) {
sweep.ph180_freq = 0.0f;
sweep.ph180_gain = 0.0f;
@ -1129,8 +1141,6 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
float sweep_time_ms = 23000;
float dwell_freq = start_frq;
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
// adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized
const float freq_co = 1.0f / attitude_control->get_input_tc();
const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f;
@ -1160,28 +1170,29 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
dwell_freq = waveform_freq_rads;
}
}
filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
filt_att_fdbk_from_velxy_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.x, -2000.0f, 2000.0f);
filt_att_fdbk_from_velxy_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.y, -2000.0f, 2000.0f);
Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x);
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
} else {
target_angle_cd = 0.0f;
trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z;
trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor;
settle_time--;
dwell_start_time_ms = now;
filt_att_fdbk_from_velxy_cd = Vector2f(0.0f,0.0f);
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
}
Vector2f trim_angle_cd;
trim_angle_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f);
trim_angle_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f);
switch (axis) {
case ROLL:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, 0.0f);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f);
command_reading = motors->get_roll();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f;
break;
case PITCH:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(filt_att_fdbk_from_velxy_cd.x, target_angle_cd + filt_att_fdbk_from_velxy_cd.y, 0.0f);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f);
command_reading = motors->get_pitch();
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f;
@ -1190,26 +1201,26 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
command_reading = motors->get_yaw();
tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f;
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f;
attitude_control->input_euler_angle_roll_pitch_yaw(filt_att_fdbk_from_velxy_cd.x, filt_att_fdbk_from_velxy_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
break;
}
if (settle_time == 0) {
filt_command_reading += alpha * (command_reading - filt_command_reading);
filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading);
filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading);
filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s());
filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s());
} else {
filt_command_reading = command_reading;
filt_gyro_reading = gyro_reading;
filt_tgt_rate_reading = tgt_rate_reading;
filt_command_reading.reset(command_reading);
filt_gyro_reading.reset(gyro_reading);
filt_tgt_rate_reading.reset(tgt_rate_reading);
}
// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading),
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading),
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()),
AP::scheduler().get_loop_period_s());
command_out = command_filt.apply((command_reading - filt_command_reading),
command_out = command_filt.apply((command_reading - filt_command_reading.get()),
AP::scheduler().get_loop_period_s());
// wait for dwell to start before determining gain and phase

34
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -132,11 +132,11 @@ private:
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_init(float start_frq, float filt_freq);
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_init(float start_frq, float filt_freq);
void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
// generates waveform for frequency sweep excitations
@ -228,25 +228,21 @@ private:
LowPassFilterFloat rate_request_cds;
LowPassFilterFloat angle_request_cd;
// variables from rate dwell test
Vector3f trim_attitude_cd;
Vector3f filt_attitude_cd;
Vector2f filt_att_fdbk_from_velxy_cd;
float filt_command_reading;
float filt_gyro_reading;
float filt_tgt_rate_reading;
float trim_command;
// variables from dwell test
LowPassFilterVector3f filt_attitude_cd;
LowPassFilterVector2f filt_att_fdbk_from_velxy_cd;
LowPassFilterFloat filt_command_reading; // filtered command reading to keep oscillation centered
LowPassFilterFloat filt_gyro_reading; // filtered gyro reading to keep oscillation centered
LowPassFilterFloat filt_tgt_rate_reading; // filtered target rate reading to keep oscillation centered
// variables from angle dwell test
float trim_yaw_tgt_reading;
float trim_yaw_heading_reading;
// Vector2f filt_att_fdbk_from_velxy_cd;
// float filt_command_reading;
// float filt_gyro_reading;
// float filt_tgt_rate_reading;
// trim variables for determining trim state prior to test starting
Vector3f trim_attitude_cd; // trim attitude before starting test
float trim_command; // trim target yaw reading before starting test
float trim_yaw_tgt_reading; // trim target yaw reading before starting test
float trim_yaw_heading_reading; // trim heading reading before starting test
LowPassFilterFloat command_filt; // filtered command
LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second
LowPassFilterFloat command_filt; // filtered command - filtering intended to remove noise
LowPassFilterFloat target_rate_filt; // filtered target rate in radians/second - filtering intended to remove noise
// sweep_data tracks the overall characteristics in the response to the frequency sweep
struct sweep_data {