Skip to content

Commit

Permalink
Copter: Autotune: Small Fixes and Feedback
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Feb 19, 2024
1 parent 227768b commit 2aac0ea
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 25 deletions.
37 changes: 19 additions & 18 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_ANGLE_MAX_RLLPIT 2.0 / 3.0 // maximum allowable angle during testing, as a ratio of angle_max
#define AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE 2.0 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max

AC_AutoTune::AC_AutoTune()
{
Expand Down Expand Up @@ -270,35 +270,35 @@ void AC_AutoTune::run()
// return true if vehicle is close to level
bool AC_AutoTune::currently_level()
{
const uint32_t now_ms = AP_HAL::millis();

// slew threshold to ensure settling time
// relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS
float threshold_mul = constrain_float((float)(now_ms - level_start_time_ms) / (float)AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0);
// abort AutoTune if we pass 2 * AUTOTUNE_LEVEL_TIMEOUT_MS
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - level_start_time_ms > 2 * AUTOTUNE_LEVEL_TIMEOUT_MS) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}

if (fabsf(ahrs_view->roll_sensor - roll_cd) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
// slew threshold to ensure sufficient settling time for aircraft unable to obtain small thresholds
// relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS
const float threshold_mul = constrain_float((float)(now_ms - level_start_time_ms) / (float)AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0);

if (fabsf(ahrs_view->roll_sensor - roll_cd) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}

if (fabsf(ahrs_view->pitch_sensor - pitch_cd) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
if (fabsf(ahrs_view->pitch_sensor - pitch_cd) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}
if (fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
if (fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}
if ((ToDeg(ahrs_view->get_gyro().x) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD) {
if ((ToDeg(ahrs_view->get_gyro().x) * 100.0f) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_CD) {
return false;
}
if ((ToDeg(ahrs_view->get_gyro().y) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD) {
if ((ToDeg(ahrs_view->get_gyro().y) * 100.0f) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_CD) {
return false;
}
if ((ToDeg(ahrs_view->get_gyro().z) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_Y_CD) {
if ((ToDeg(ahrs_view->get_gyro().z) * 100.0f) > threshold_mul * AUTOTUNE_LEVEL_RATE_Y_CD) {
return false;
}
return true;
Expand Down Expand Up @@ -356,18 +356,19 @@ void AC_AutoTune::control_attitude()
// Initialize test-specific variables
switch (axis) {
case ROLL:
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT;
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor;
break;
case PITCH:
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT;
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor;
break;
case YAW:
case YAW_D:
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW;
// Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
break;
Expand All @@ -387,16 +388,16 @@ void AC_AutoTune::control_attitude()
test_run(axis, direction_sign);

// Check for failure causing reverse response
if (lean_angle <= -attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT) {
if (lean_angle <= -attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE) {
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
level_start_time_ms = now;
}

// protect from roll over
float resultant_angle = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch()));
if (resultant_angle > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT) {
const float resultant_angle_cd = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch()));
if (resultant_angle_cd > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE) {
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
Expand Down
8 changes: 4 additions & 4 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,10 @@

#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000

#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT 1.0 / 4.0 // minimum target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_RLLPIT 1.0 / 2.0 // target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW 1.0 / 8.0 // minimum target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_YAW 2.0 / 3.0 // target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE 1.0 / 4.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE 1.0 / 2.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE 1.0 / 8.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_YAW_SCALE 2.0 / 3.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step

class AC_AutoTune
{
Expand Down
6 changes: 3 additions & 3 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1116,22 +1116,22 @@ void AC_AutoTune_Multi::twitch_test_init()
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, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
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, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
}
case YAW:
case YAW_D: {
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.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_YAW, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE);
if (axis == YAW_D) {
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz()*2.0f);
} else {
Expand Down

0 comments on commit 2aac0ea

Please sign in to comment.