Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: loosen level threshold after 2s

Browse Source
This commit is contained in:
Andrew Tridgell 2018-12-22 11:20:45 +11:00
parent d0bc1520f6
commit 460d697678
1 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -431,35 +431,43 @@ bool AC_AutoTune::check_level(const LEVEL_ISSUE issue, const float current, cons
bool AC_AutoTune::currently_level() bool AC_AutoTune::currently_level()
{ {
float threshold_mul = 1.0;
if (AP_HAL::millis() - 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;
}
if (!check_level(LEVEL_ISSUE_ANGLE_ROLL, if (!check_level(LEVEL_ISSUE_ANGLE_ROLL,
fabsf(ahrs_view->roll_sensor - roll_cd), fabsf(ahrs_view->roll_sensor - roll_cd),
AUTOTUNE_LEVEL_ANGLE_CD)) { threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false; return false;
} }
if (!check_level(LEVEL_ISSUE_ANGLE_PITCH, if (!check_level(LEVEL_ISSUE_ANGLE_PITCH,
fabsf(ahrs_view->pitch_sensor - pitch_cd), fabsf(ahrs_view->pitch_sensor - pitch_cd),
AUTOTUNE_LEVEL_ANGLE_CD)) { threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false; return false;
} }
if (!check_level(LEVEL_ISSUE_ANGLE_YAW, if (!check_level(LEVEL_ISSUE_ANGLE_YAW,
fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)), fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)),
AUTOTUNE_LEVEL_ANGLE_CD)) { threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false; return false;
} }
if (!check_level(LEVEL_ISSUE_RATE_ROLL, if (!check_level(LEVEL_ISSUE_RATE_ROLL,
(ToDeg(ahrs_view->get_gyro().x) * 100.0f), (ToDeg(ahrs_view->get_gyro().x) * 100.0f),
AUTOTUNE_LEVEL_RATE_RP_CD)) { threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
return false; return false;
} }
if (!check_level(LEVEL_ISSUE_RATE_PITCH, if (!check_level(LEVEL_ISSUE_RATE_PITCH,
(ToDeg(ahrs_view->get_gyro().y) * 100.0f), (ToDeg(ahrs_view->get_gyro().y) * 100.0f),
AUTOTUNE_LEVEL_RATE_RP_CD)) { threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
return false; return false;
} }
if (!check_level(LEVEL_ISSUE_RATE_YAW, if (!check_level(LEVEL_ISSUE_RATE_YAW,
(ToDeg(ahrs_view->get_gyro().z) * 100.0f), (ToDeg(ahrs_view->get_gyro().z) * 100.0f),
AUTOTUNE_LEVEL_RATE_Y_CD)) { threshold_mul*AUTOTUNE_LEVEL_RATE_Y_CD)) {
return false; return false;
} }
return true; return true;
@ -493,8 +501,7 @@ void AC_AutoTune::control_attitude()
} }
// if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step
if (now - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS || if (now - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS) {
now - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch"); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch");
// initiate variables for next step // initiate variables for next step
step = TWITCHING; step = TWITCHING;