From d4987be5f292f01ce0364d63ae1319497031974c Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 16 Sep 2019 21:54:38 -0300 Subject: [PATCH] Sub: rate-limit init_motor_test() calls --- ArduSub/motors.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index e16c37a32c..e3621a04e3 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -78,10 +78,19 @@ bool Sub::handle_do_motor_test(mavlink_command_long_t command) { last_do_motor_test_ms = AP_HAL::millis(); // If we are not already testing motors, initialize test + static uint32_t tLastInitializationFailed = 0; if(!ap.motor_test) { - if (!init_motor_test()) { - gcs().send_text(MAV_SEVERITY_WARNING, "motor test initialization failed!"); - return false; // init fail + // Do not allow initializations attempt under 2 seconds + // If one fails, we need to give the user time to fix the issue + // instead of spamming error messages + if (AP_HAL::millis() > (tLastInitializationFailed + 2000)) { + if (!init_motor_test()) { + gcs().send_text(MAV_SEVERITY_WARNING, "motor test initialization failed!"); + tLastInitializationFailed = AP_HAL::millis(); + return false; // init fail + } + } else { + return false; } }