From 9d7291f28a94bdf3d759176efb1c53c0f1505b33 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Fri, 1 Oct 2021 23:07:31 -0400 Subject: [PATCH] AC_AutoTune: removed pilot control during angle P tuning, added vel feedback --- libraries/AC_AutoTune/AC_AutoTune.cpp | 30 ++++++++++++++--------- libraries/AC_AutoTune/AC_AutoTune.h | 3 --- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 10 -------- libraries/AC_AutoTune/AC_AutoTune_Multi.h | 3 --- 4 files changed, 18 insertions(+), 28 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index ecdfc8b1c3..dbdeadb25a 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -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; } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index f5ede905b6..d4c7fa08ba 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -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; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 3c4fdd6059..99bc5747a8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -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; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 6259df7ab2..87fe780c33 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -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; }