Skip to content

Commit

Permalink
Copter: Autotune: Level requirement improvements
Browse files Browse the repository at this point in the history
squash level
  • Loading branch information
lthall committed Feb 19, 2024
1 parent 6d36a5a commit 51e15a5
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 28 deletions.
55 changes: 29 additions & 26 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
# define AUTOTUNE_LEVEL_RATE_RP_CD 500 // rate which qualifies as level for roll and pitch
#endif
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level before starting next test
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#define AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE 2.0 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max
Expand All @@ -41,6 +41,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
ahrs_view = _ahrs_view;
inertial_nav = _inertial_nav;
motors = AP_Motors::get_singleton();
const uint32_t now = AP_HAL::millis();

// exit immediately if motor are not armed
if ((motors == nullptr) || !motors->armed()) {
Expand Down Expand Up @@ -71,8 +72,8 @@ bool AC_AutoTune::init_internals(bool _use_poshold,

// we are restarting tuning so restart where we left off
step = WAITING_FOR_LEVEL;
step_start_time_ms = AP_HAL::millis();
level_start_time_ms = step_start_time_ms;
step_start_time_ms = now;
level_start_time_ms = now;
// reset gains to tuning-start gains (i.e. low I term)
load_gains(GAIN_INTRA_TEST);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART);
Expand Down Expand Up @@ -270,39 +271,35 @@ void AC_AutoTune::run()
// return true if vehicle is close to level
bool AC_AutoTune::currently_level()
{
float threshold_mul = 1.0;

uint32_t now_ms = AP_HAL::millis();
if (now_ms - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
// after a long wait we use looser threshold, to allow tuning
// with poor initial gains
threshold_mul *= 2;
// 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);
}

// display warning if vehicle fails to level
if ((now_ms - level_start_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS) &&
(now_ms - level_fail_warning_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: failing to level, please tune manually");
level_fail_warning_time_ms = now_ms;
}
// 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) {
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 @@ -395,13 +392,17 @@ void AC_AutoTune::control_attitude()
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
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;
level_start_time_ms = now;
}

#if HAL_LOGGING_ENABLED
Expand Down Expand Up @@ -551,7 +552,7 @@ void AC_AutoTune::control_attitude()
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
level_start_time_ms = step_start_time_ms;
level_start_time_ms = now;
step_time_limit_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS;
break;
}
Expand All @@ -561,6 +562,8 @@ void AC_AutoTune::control_attitude()
// called before tuning starts to backup original gains
void AC_AutoTune::backup_gains_and_initialise()
{
const uint32_t now = AP_HAL::millis();

// initialise state because this is our first time
if (roll_enabled()) {
axis = ROLL;
Expand All @@ -580,10 +583,10 @@ void AC_AutoTune::backup_gains_and_initialise()
// start at the beginning of tune sequence
next_tune_type(tune_type, true);

positive_direction = false;
step = WAITING_FOR_LEVEL;
step_start_time_ms = AP_HAL::millis();
level_start_time_ms = step_start_time_ms;
positive_direction = false;
step_start_time_ms = now;
level_start_time_ms = now;
step_scaler = 1.0f;

desired_yaw_cd = ahrs_view->yaw_sensor;
Expand Down
3 changes: 1 addition & 2 deletions libraries/AC_AutoTune/AC_AutoTune.h
Original file line number Diff line number Diff line change
Expand Up @@ -273,6 +273,7 @@ class AC_AutoTune
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
Expand Down Expand Up @@ -330,8 +331,6 @@ class AC_AutoTune

// variables
uint32_t override_time; // the last time the pilot overrode the controls
uint32_t level_start_time_ms; // start time of waiting for level
uint32_t level_fail_warning_time_ms; // last time level failure warning message was sent to GCS

// time in ms of last pilot override warning
uint32_t last_pilot_override_warning;
Expand Down
3 changes: 3 additions & 0 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -542,6 +542,7 @@ void AC_AutoTune_Multi::twitching_test_rate(float rate, float rate_target_max, f
// 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)
{
const uint32_t now = AP_HAL::millis();
if (angle >= angle_max) {
if (is_equal(rate, meas_rate_min) && step_scaler > 0.5f) {
// we have reached the angle limit before completing the measurement of maximum and minimum
Expand All @@ -550,6 +551,8 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
// 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;
} else {
step = UPDATE_GAINS;
}
Expand Down

0 comments on commit 51e15a5

Please sign in to comment.