diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 6dac22cc742157..c35845c3875962 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -134,6 +134,9 @@ void AC_AutoTune::send_step_string() case UPDATE_GAINS: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains"); return; + case ABORT: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test"); + return; case TESTING: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing"); return; @@ -340,13 +343,6 @@ void AC_AutoTune::control_attitude() step_start_time_ms = now; step_time_limit_ms = get_testing_step_timeout_ms(); // set gains to their to-be-tested values - twitch_first_iter = true; - test_rate_max = 0.0f; - test_rate_min = 0.0f; - test_angle_max = 0.0f; - test_angle_min = 0.0f; - rotation_rate_filt.reset(0.0f); - rate_max = 0.0f; load_gains(GAIN_TEST); } else { // when waiting for level we use the intra-test gains @@ -356,18 +352,15 @@ void AC_AutoTune::control_attitude() // Initialize test-specific variables switch (axis) { case ROLL: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; start_angle = ahrs_view->roll_sensor; break; case PITCH: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; start_angle = ahrs_view->pitch_sensor; break; case YAW: case YAW_D: - angle_finish = target_angle_max_y_cd(); start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f; start_angle = ahrs_view->yaw_sensor; break; @@ -537,7 +530,9 @@ void AC_AutoTune::control_attitude() } } } + FALLTHROUGH; + case ABORT: if (axis == YAW || axis == YAW_D) { attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 4ff35df6dd8994..1c87a9599f9aa4 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -197,7 +197,8 @@ class AC_AutoTune enum StepType { WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement - UPDATE_GAINS = 2 // autotune has completed a test and is updating the gains based on the results + UPDATE_GAINS = 2, // autotune has completed a test and is updating the gains based on the results + ABORT = 3 // load normal gains and return to WAITING_FOR_LEVEL }; // mini steps performed while in Tuning mode, Testing step @@ -259,26 +260,18 @@ class AC_AutoTune bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) StepType step; // see StepType for what steps are performed TuneType tune_type; // see TuneType - bool ignore_next; // true = ignore the next test bool twitch_first_iter; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) uint8_t axes_completed; // bitmask of completed axes - float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step-multi only - float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step-multi only - float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step-multi only - float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step-multi only uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks) uint32_t step_time_limit_ms; // time limit of current autotune process uint32_t level_start_time_ms; // start time of waiting for level int8_t counter; // counter for tuning gains - float target_rate; // target rate-multi only - float target_angle; // target angle-multi only - float start_rate; // start rate - parent and multi float start_angle; // start angle - float rate_max; // maximum rate variable - parent and multi + float start_rate; // start rate - parent and multi float test_accel_max; // maximum acceleration variable - float step_scaler; // scaler to reduce maximum target step - parent and multi - float angle_finish; // Angle that test is aborted- parent and multi + float desired_yaw_cd; // yaw heading during tune - parent and Tradheli + float step_scaler; // scaler to reduce maximum target step - parent and multi LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 940cd48e6f2fcf..5b7aedf7a0492f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -573,7 +573,6 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_ // update min and max and test for end conditions void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min) { - const uint32_t now = AP_HAL::millis(); if (angle >= angle_max) { if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) { // we have reached the angle limit before completing the measurement of maximum and minimum @@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); } // ignore result and start test again - step = WAITING_FOR_LEVEL; - positive_direction = twitch_reverse_direction(); - step_start_time_ms = now; - level_start_time_ms = now; + step = ABORT; } else { step = UPDATE_GAINS; } @@ -653,11 +649,10 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl } // twitching_measure_acceleration - measure rate of change of measurement -void AC_AutoTune_Multi::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const +void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float meas_rate_max) const { - if (rate_measurement_max < rate_measurement) { - rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time_ms); + if (is_equal(rate, meas_rate_max)) { + accel_average = (1000.0*test_rate_max)/(AP_HAL::millis() - step_start_time_ms); } } @@ -1189,21 +1184,24 @@ void AC_AutoTune_Multi::twitch_test_init() float target_max_rate; switch (axis) { case ROLL: { - target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); - target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, target_angle_min_rp_cd(), target_angle_max_rp_cd()); - rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f); + angle_abort = target_angle_max_rp_cd(); + target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); + rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz() * 2.0); break; } case PITCH: { - target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); - target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, target_angle_min_rp_cd(), target_angle_max_rp_cd()); - rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f); + angle_abort = target_angle_max_rp_cd(); + target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); + rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz() * 2.0); break; } case YAW: case YAW_D: { + angle_abort = target_angle_max_y_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw() * 0.75) * 100.0, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw() * 0.75) * 100.0, target_angle_min_y_cd(), target_angle_max_y_cd()); @@ -1221,6 +1219,12 @@ void AC_AutoTune_Multi::twitch_test_init() } else { rotation_rate_filt.reset(0); } + twitch_first_iter = true; + test_rate_max = 0.0; + test_rate_min = 0.0; + test_angle_max = 0.0; + test_angle_min = 0.0; + rotation_rate_filt.reset(0.0); } @@ -1312,18 +1316,18 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign case RD_UP: case RD_DOWN: twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case RP_UP: - twitching_test_rate(lean_angle, rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case SP_DOWN: case SP_UP: - twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, rate_max); + twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, test_rate_max); break; case RFF_UP: case MAX_GAINS: diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 3650c85bff1730..a61dbe1f94d63b 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -156,7 +156,7 @@ class AC_AutoTune_Multi : public AC_AutoTune void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); // measure acceleration during twitch test - void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const; + void twitching_measure_acceleration(float &accel_average, float rate, float meas_rate_max) const; // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P @@ -182,9 +182,18 @@ class AC_AutoTune_Multi : public AC_AutoTune void report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel) const; // parameters - AP_Int8 axis_bitmask; // axes to be tuned - AP_Float aggressiveness; // aircraft response aggressiveness to be tuned - AP_Float min_d; // minimum rate d gain allowed during tuning + AP_Int8 axis_bitmask; // axes to be tuned + AP_Float aggressiveness; // aircraft response aggressiveness to be tuned + AP_Float min_d; // minimum rate d gain allowed during tuning + + bool ignore_next; // ignore the results of the next test when true + float target_angle; // target angle for the test + float target_rate; // target rate for the test + float angle_abort; // Angle that test is aborted + float test_rate_min; // the minimum angular rate achieved during TESTING_RATE + float test_rate_max; // the maximum angular rate achieved during TESTING_RATE + float test_angle_min; // the minimum angle achieved during TESTING_ANGLE + float test_angle_max; // the maximum angle achieved during TESTING_ANGLE }; #endif // AC_AUTOTUNE_ENABLED