Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
82 changes: 82 additions & 0 deletions Tools/autotest/quadplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -708,6 +708,87 @@ def QAUTOTUNE(self):
self.wait_disarmed()
self.reboot_sitl() # far from home

def QAUTOTUNEYawRepoint(self):
'''check QAutoTune survives a slow position-hold yaw re-point'''
# Each tuning twitch displaces the vehicle a little; beyond 5m
# of drift autotune's position hold commands a yaw re-point
# along the drift direction (+90deg while tuning pitch). With
# a low yaw target acceleration that re-point's traverse takes
# longer than the failed-to-level timeout while remaining below
# the yaw-rate-based timeout deferral threshold, so autotune
# must not run the level timeout while the yaw target is still
# slewing towards the desired yaw.
self.set_parameters({
"Q_A_ACC_Y_MAX": 5,
"Q_AUTOTUNE_AXES": 3, # roll and pitch only
# adjust tune so QAUTOTUNE can cope:
"Q_AUTOTUNE_AGGR": 0.1,
"Q_AUTOTUNE_MIN_D": 0.0004,
"Q_A_RAT_RLL_P": 0.15,
"Q_A_RAT_RLL_I": 0.25,
"Q_A_RAT_RLL_D": 0.002,
"Q_A_RAT_PIT_P": 0.15,
"Q_A_RAT_PIT_I": 0.25,
"Q_A_RAT_PIT_D": 0.002,
"Q_A_RAT_YAW_P": 0.18,
"Q_A_RAT_YAW_I": 0.018,
"Q_A_ANG_RLL_P": 4.5,
"Q_A_ANG_PIT_P": 4.5,
})

self.takeoff(15, mode='GUIDED')
self.hover()
self.change_mode("QLOITER")

# record how far the vehicle drifts and yaws away from where
# autotune starts, so the test fails if the re-point it exists
# for stops happening, rather than passing without exercising it
start_pos = self.assert_receive_message('GLOBAL_POSITION_INT')
start_yaw = math.degrees(self.assert_receive_message('ATTITUDE').yaw)
extremes = {"drift": 0.0, "yaw_change": 0.0}

def record_extremes(mav, m):
t = m.get_type()
if t == 'GLOBAL_POSITION_INT':
extremes["drift"] = max(extremes["drift"], self.get_distance_int(start_pos, m))
elif t == 'ATTITUDE':
change = abs((math.degrees(m.yaw) - start_yaw + 180) % 360 - 180)
extremes["yaw_change"] = max(extremes["yaw_change"], change)

self.context_push()
self.install_message_hook_context(record_extremes)
tstart = self.get_sim_time()
self.context_collect('STATUSTEXT')
self.change_mode("QAUTOTUNE")
self.wait_text(
"AutoTune: (Success|Failed to level).*",
timeout=5000,
check_context=True,
regex=True,
)
self.context_pop()
self.progress("drifted up to %.1fm and yawed up to %.0fdeg from the start" %
(extremes["drift"], extremes["yaw_change"]))
if self.re_match.group(1) != "Success":
raise NotAchievedException("autotune did not succeed")
# position hold only re-points beyond 5m of drift, and then by
# at least 90deg while tuning pitch (seen: 11.2m, 140deg)
if extremes["drift"] < 5:
raise NotAchievedException("only drifted %.1fm; no yaw re-point was exercised" %
extremes["drift"])
if extremes["yaw_change"] < 90:
raise NotAchievedException("only yawed %.0fdeg; no large yaw re-point was exercised" %
extremes["yaw_change"])
self.progress("AUTOTUNE OK (%u seconds)" % (self.get_sim_time() - tstart))

# leave QAUTOTUNE before disarming so the tuned gains are not saved
self.change_mode("QLOITER")
self.disarm_vehicle(force=True)
self.reboot_sitl() # may have drifted far from home

def hover(self, hover_throttle=1500):
self.set_rc(3, hover_throttle)

def takeoff(self, height, mode, timeout=30):
"""climb to specified height and set throttle to 1500"""
self.set_current_waypoint(0, check_afterwards=False)
Expand Down Expand Up @@ -4647,6 +4728,7 @@ def tests1c(self):
# first, on a fresh SITL: straight after PilotYaw it failed to level 5 times in 10
# (PR #33628 fixes the underlying AutoTune level-timeout race)
self.QAUTOTUNE,
self.QAUTOTUNEYawRepoint,
self.AHRSSwitchBackendResets,
self.PilotYaw,
self.TestLogDownloadWrap,
Expand Down
46 changes: 45 additions & 1 deletion libraries/AC_AutoTune/AC_AutoTune.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#define AUTOTUNE_LEVEL_RATE_Y_RADS radians(7.5) // rate which qualifies as level for yaw
#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_DEFER_MAX_MS 20000 // longest a yaw slew may postpone the level time out
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users

AC_AutoTune::AC_AutoTune()
Expand Down Expand Up @@ -68,6 +69,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
step = Step::WAITING_FOR_LEVEL;
step_start_time_ms = now_ms;
level_start_time_ms = now_ms;
level_wait_start_ms = now_ms;
// Reload gains with low I-term and restart logging
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART);
update_gcs(AUTOTUNE_MESSAGE_STARTED);
Expand Down Expand Up @@ -275,6 +277,7 @@ void AC_AutoTune::run()
step = Step::WAITING_FOR_LEVEL;
step_start_time_ms = now_ms;
level_start_time_ms = now_ms;
level_wait_start_ms = now_ms;
// TODO: Consider using our current target.
desired_yaw_rad = ahrs_view->get_yaw_rad(); // Reset yaw reference
}
Expand Down Expand Up @@ -328,10 +331,49 @@ bool AC_AutoTune::currently_level()
{
// abort AutoTune if we pass 2 * AUTOTUNE_LEVEL_TIMEOUT_MS
const uint32_t now_ms = AP_HAL::millis();
if (fabsf(attitude_control->get_rate_ef_target_rads().z) > 0.5 * attitude_control->get_slew_yaw_max_rads()) {
// the yaw-slew deferrals below postpone the abort, but only for so
// long: a yaw target which never settles - e.g. position hold
// re-pointing along a drift bearing which keeps moving - would
// otherwise keep us waiting for level forever
const bool may_defer_timeout = now_ms - level_wait_start_ms < AUTOTUNE_LEVEL_DEFER_MAX_MS;
if (may_defer_timeout && fabsf(attitude_control->get_rate_ef_target_rads().z) > 0.5 * attitude_control->get_slew_yaw_max_rads()) {
// reset if the target yaw rate is above half the slew rate
level_start_time_ms = now_ms;
}
if (may_defer_timeout && fabsf(wrap_PI(attitude_control->get_att_target_euler_rad().z - desired_yaw_rad)) > AUTOTUNE_LEVEL_ANGLE_RAD) {
// the attitude controller is still slewing its yaw target towards

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Too much?

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

This is all correct but it does basically what the check above does but with a smaller window in some cases.

// desired_yaw_rad (e.g. after position-hold commands a large
// re-point); the vehicle cannot be at the desired yaw until the
// commanded target arrives there, so do not run the level timeout.
// A target-rate check does not cover this: a shaped re-point's
// spin-up and spin-down tails run below any useful rate
// threshold while the target is still far from desired, and
// with low yaw acceleration those tails alone can exceed the
// timeout (measured: rate-target-based deferral at
// AUTOTUNE_LEVEL_RATE_Y_RADS still failed the low-acceleration
// repro 2 runs in 4).
level_start_time_ms = now_ms;
last_yaw_slew_ms = now_ms;
}
// the airframe lags the target it is given, so the yaw is not
// settled the moment the target arrives at desired_yaw_rad: a
// shaped re-point leaves the vehicle still turning, and it swings
// through desired and back before it is level. Measured on the
// QAUTOTUNEYawRepoint repro: 12 degrees of overshoot taking 6s to
// decay, against the 6s timeout, which aborted the tune while the
// vehicle was still converging. Allow the time it takes to arrest
// a full-rate yaw before the timeout starts counting; that is a
// fraction of a second on a normally-configured vehicle, and is
// only long where the yaw acceleration limit is low, which is
// exactly where the overshoot is slow to decay.
const float yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
if (may_defer_timeout && is_positive(yaw_accel_radss)) {
const uint32_t settle_ms = MIN(1000.0 * attitude_control->get_slew_yaw_max_rads() / yaw_accel_radss,
(float)AUTOTUNE_LEVEL_DEFER_MAX_MS);
if (now_ms - last_yaw_slew_ms < settle_ms) {
level_start_time_ms = now_ms;
}
}
if (now_ms - level_start_time_ms > 3 * AUTOTUNE_LEVEL_TIMEOUT_MS) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
mode = TuneMode::FAILED;
Expand Down Expand Up @@ -553,6 +595,7 @@ void AC_AutoTune::control_attitude()
positive_direction = reverse_test_direction();
step_start_time_ms = now_ms;
level_start_time_ms = now_ms;
level_wait_start_ms = now_ms;
step_timeout_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS;
break;
}
Expand Down Expand Up @@ -587,6 +630,7 @@ void AC_AutoTune::backup_gains_and_initialise()
positive_direction = false;
step_start_time_ms = now_ms;
level_start_time_ms = now_ms;
level_wait_start_ms = now_ms;
step_scaler = 1.0f;

desired_yaw_rad = ahrs_view->get_yaw_rad();
Expand Down
2 changes: 2 additions & 0 deletions libraries/AC_AutoTune/AC_AutoTune.h
Original file line number Diff line number Diff line change
Expand Up @@ -275,6 +275,8 @@ class AC_AutoTune
uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks)
uint32_t step_timeout_ms; // time limit of current autotune process
uint32_t level_start_time_ms; // start time of waiting for level
uint32_t level_wait_start_ms; // start time of waiting for level, not moved by the yaw-slew deferrals
uint32_t last_yaw_slew_ms; // last time the yaw target was away from desired_yaw_rad
int8_t success_counter; // counter for tuning gains
float start_angle_rad; // start angle in radians
float start_rate_rads; // start rate in radians/second - parent and multi
Expand Down
Loading