diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 6dac22cc74..40e2e9e2e2 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -340,13 +340,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 +349,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; @@ -539,6 +529,7 @@ void AC_AutoTune::control_attitude() } if (axis == YAW || axis == YAW_D) { + // todo: check to make sure we need this 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 510fd6c59f..fb4233a23e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -259,24 +259,15 @@ protected: 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_angle; // start angle float start_rate; // start rate - parent and multi - float rate_max; // maximum rate variable - parent and multi float test_accel_max; // maximum acceleration variable - 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 diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index d3ffdefe1a..89c1a45b24 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -60,15 +60,15 @@ #define AUTOTUNE_RP_MAX 2.0 // maximum Rate P value #define AUTOTUNE_SP_MAX 40.0 // maximum Stab P value #define AUTOTUNE_SP_MIN 0.5 // maximum Stab P value -#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw +#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw #define AUTOTUNE_Y_FILT_FREQ 10.0 // Autotune filter frequency when testing Yaw #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2 // The margin below the target that we tune D in -#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning #define AUTOTUNE_RP_BACKOFF 1.0 // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning -#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9 // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0 // back off from maximum acceleration -#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration // roll and pitch axes #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step @@ -653,11 +653,11 @@ 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 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 (rate_max < rate) { + rate_max = rate; + accel_average = (1000.0 * rate_max) / (AP_HAL::millis() - step_start_time_ms); } } @@ -1189,6 +1189,7 @@ void AC_AutoTune_Multi::twitch_test_init() float target_max_rate; switch (axis) { case ROLL: { + 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()); @@ -1196,6 +1197,7 @@ void AC_AutoTune_Multi::twitch_test_init() break; } case PITCH: { + 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()); @@ -1204,6 +1206,7 @@ void AC_AutoTune_Multi::twitch_test_init() } 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()); @@ -1217,11 +1220,16 @@ void AC_AutoTune_Multi::twitch_test_init() } if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + // todo: consider if this should be done for other axis rotation_rate_filt.reset(start_rate); } else { - rotation_rate_filt.reset(0); + rotation_rate_filt.reset(0.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; } //run twitch test @@ -1312,18 +1320,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 3308ae5d8a..e580e4afcb 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -156,7 +156,7 @@ private: 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 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 @@ -185,6 +185,14 @@ private: 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