Sub: rate-limit init_motor_test() calls

This commit is contained in:
Willian Galvani 2019-09-16 21:54:38 -03:00 committed by Jacob Walser
parent d23ad705e1
commit ed6d7c434b
1 changed files with 12 additions and 3 deletions

View File

@ -197,11 +197,20 @@ 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) {
// 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;
}
}
float motor_number = command.param1;