Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AC_Autotune: Clean up Multi Variables and non functional changes #27370

Merged
merged 2 commits into from
Jul 26, 2024
Merged

AC_Autotune: Clean up Multi Variables and non functional changes #27370

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9f307c5
AC_Autotune: Clean up Multi Variables and non functional changes
lthall Jul 21, 2024
04a0bd0
AC_Autotune: Add ABORT state for consistency between tests
lthall Jul 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 7 additions & 10 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Open in desktop
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,8 +530,11 @@ void AC_AutoTune::control_attitude()
}
}
}
FALLTHROUGH;

case ABORT:
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);
}

Expand Down Expand Up @@ -594,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise()
*/
void AC_AutoTune::load_gains(enum GainType gain_type)
{
// todo: add previous setting so gains are not loaded on each loop.
switch (gain_type) {
case GAIN_ORIGINAL:
load_orig_gains();
Expand Down
10 changes: 1 addition & 9 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,7 @@ class AC_AutoTune
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
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,24 +260,15 @@ 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_angle; // start angle
float start_rate; // start rate - parent and multi
float rate_max; // maximum rate variable - parent and multi
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

somewhat surprisingly AC_AutoTune_Heli also defines a "rate_max" variable but it is a parameter. I'm slightly surprised that the compiler didn't catch this "shadowing" but in any case it's good to get rid of it.

I think @bnsgeyer should confirm that Heli autotune seems to work OK

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

Expand Down
50 changes: 27 additions & 23 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down 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,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);
}
}

Expand Down Expand Up @@ -1189,13 +1185,15 @@ 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());
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz() * 2.0);
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());
Expand All @@ -1204,6 +1202,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());
Expand All @@ -1217,11 +1216,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
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
10 changes: 9 additions & 1 deletion libraries/AC_AutoTune/AC_AutoTune_Multi.h
View file
Open in desktop
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 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 Down Expand Up @@ -185,6 +185,14 @@ class AC_AutoTune_Multi : public AC_AutoTune
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
Loading