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; } }