From 25beab4c82a31d061ebe862e49b669006943c257 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Feb 2021 11:41:27 +0900 Subject: [PATCH] AC_AutoTune: add failure to level warning --- libraries/AC_AutoTune/AC_AutoTune.cpp | 13 +++++++++++-- libraries/AC_AutoTune/AC_AutoTune.h | 1 + 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 9762e15576..30e0bf3d95 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -52,7 +52,8 @@ #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 @@ -439,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)) { diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 608a57f069..a279f434ba 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -179,6 +179,7 @@ private: 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