From 2e9bd43c4900b177bb4a7ccf9053a0101814f471 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 0f37852689..89e6566a1a 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -82,10 +82,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; } }