Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Autotune: Clean up Multi Variables and non functional changes

Browse Source
This commit is contained in:
Leonard Hall 2024-07-21 20:58:24 +09:30 committed by Peter Barker
parent 1f60ca3342
commit bea2c5b59b
4 changed files with 36 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -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);
}

9
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -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

44
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -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:

10
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -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