Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AutoTune: tighten wait-for-level criteria for Copters #16471

Merged
merged 3 commits into from Feb 6, 2021
Merged

AutoTune: tighten wait-for-level criteria for Copters #16471

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5fb5b7a
AC_AutoTune: level criteria is 5deg for plane and 2.5 for all others
rmackay9 Feb 1, 2021
fe8d62d
AC_AutoTune: add failure to level warning
rmackay9 Feb 1, 2021
3712e55
AC_AutoTune: report step in lower case
rmackay9 Feb 1, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
30 changes: 22 additions & 8 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Open in desktop
Expand Up @@ -43,11 +43,17 @@

#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000U // timeout for tuning mode's testing step
#define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level
#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg)
# define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec)
#else
# define AUTOTUNE_LEVEL_ANGLE_CD 250 // angle which qualifies as level
# 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_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level (relaxes criteria)
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term
#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term
#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
Expand Down Expand Up @@ -252,13 +258,13 @@ void AC_AutoTune::send_step_string()
}
switch (step) {
case WAITING_FOR_LEVEL:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: WFL (%s) (%f > %f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Leveling (%s %4.1f > %4.1f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f));
return;
case UPDATE_GAINS:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: UPDATING_GAINS");
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
return;
case TWITCHING:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: TWITCHING");
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitching");
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
Expand Down Expand Up @@ -391,7 +397,7 @@ void AC_AutoTune::run()
}
if (pilot_override) {
if (now - last_pilot_override_warning > 1000) {
gcs().send_text(MAV_SEVERITY_INFO, "AUTOTUNE: pilot overrides active");
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active");
last_pilot_override_warning = now;
}
}
Expand Down Expand Up @@ -434,12 +440,20 @@ bool AC_AutoTune::currently_level()
{
float threshold_mul = 1.0;

if (AP_HAL::millis() - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
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;
}

// 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, manual tune may be required");
level_fail_warning_time_ms = now_ms;
}

if (!check_level(LevelIssue::ANGLE_ROLL,
fabsf(ahrs_view->roll_sensor - roll_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
Expand Down
1 change: 1 addition & 0 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Open in desktop
Expand Up @@ -179,6 +179,7 @@ class AC_AutoTune {
float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step
uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks)
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
uint32_t step_time_limit_ms; // time limit of current autotune process
int8_t counter; // counter for tuning gains
float target_rate, start_rate; // target and start rate
Expand Down