AC_AutoTune: add failure to level warning

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)) {

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