diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index ec71c44fd02d91..4f627cbf5dfc06 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -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() { @@ -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; @@ -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; @@ -387,7 +388,7 @@ 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; @@ -395,8 +396,8 @@ void AC_AutoTune::control_attitude() } // 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; diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 1d13aff855e4c0..e2f15dab7a3aa5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -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 { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 5f60bbadc878eb..f6984a541f7b44 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1116,14 +1116,14 @@ 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; } @@ -1131,7 +1131,7 @@ void AC_AutoTune_Multi::twitch_test_init() 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 {