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

AC_AutoTune: removed pilot control during angle P tuning, added vel feedback

This commit is contained in:
Bill Geyer 2021-10-01 23:07:31 -04:00 committed by Bill Geyer
parent e780687f5f
commit 9d7291f28a
4 changed files with 18 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -281,11 +281,7 @@ void AC_AutoTune::run()
// get pilot desired climb rate
const float target_climb_rate_cms = get_pilot_desired_climb_rate_cms();
bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd);
// allow pilots to make inputs less than 5 deg in pitch and roll
if (allow_pilot_rp_input() && !pilot_override && fabsf(target_roll_cd) < 500 && fabsf(target_pitch_cd) < 500) {
zero_rp_input = true;
}
const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd);
const uint32_t now = AP_HAL::millis();
if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) {
@ -319,7 +315,7 @@ void AC_AutoTune::run()
last_pilot_override_warning = now;
}
}
if (zero_rp_input && !allow_pilot_rp_input()) {
if (zero_rp_input) {
// pilot input on throttle and yaw will still use position hold if enabled
get_poshold_attitude(target_roll_cd, target_pitch_cd, desired_yaw_cd);
}
@ -1951,6 +1947,7 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d
static float filt_command_reading;
static float filt_gyro_reading;
static float filt_tgt_rate_reading;
static Vector2f filt_att_fdbk_from_velxy_cd;
const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq);
@ -1964,6 +1961,13 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d
cycle_time_ms = 1000.0f * 6.28f / dwell_freq;
}
// body frame calculation of velocity
Vector3f velocity_ned, velocity_bf;
if (ahrs_view->get_velocity_NED(velocity_ned)) {
velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
}
if (settle_time == 0) {
// give gentler start for the dwell
if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) {
@ -1976,26 +1980,28 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d
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);
} 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);
}
float target_roll_cd, target_pitch_cd, target_yaw_rate_cds;
get_pilot_desired_rp_yrate_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds);
switch (axis) {
case ROLL:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll_cd + target_angle_cd, target_pitch_cd, 0.0f);
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);
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(target_roll_cd, target_pitch_cd + target_angle_cd, 0.0f);
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);
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;
@ -2004,7 +2010,7 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d
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(target_roll_cd, target_pitch_cd, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
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);
break;
}

3
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -174,9 +174,6 @@ protected:
// 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;

10
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -99,16 +99,6 @@ protected:
// 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;

3
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -80,9 +80,6 @@ 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; }