Skip to content

Commit

Permalink
AC_Autotune: Clean up Multi Variables and non functional changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall @peterbarker
lthall authored and peterbarker committed Jun 24, 2024
1 parent 5ab2aaf commit 62b61bb
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 50 deletions.
15 changes: 5 additions & 10 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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);
}
Expand Down
17 changes: 5 additions & 12 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand Down
52 changes: 28 additions & 24 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
}
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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());
Expand All @@ -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);

}

Expand Down Expand Up @@ -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:
Expand Down
17 changes: 13 additions & 4 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

0 comments on commit 62b61bb

Please sign in to comment.