diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp index 76a8ee1cd0..4f6cc6b5a9 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.cpp +++ b/libraries/AP_KDECAN/AP_KDECAN.cpp @@ -628,13 +628,12 @@ void AP_KDECAN::update() bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len) { if (!_enum_sem.take(1)) { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get enumeration semaphore on read"); - snprintf(reason, reason_len ,"Enumeration state unknown"); + snprintf(reason, reason_len ,"enumeration state unknown"); return false; } if (_enumeration_state != ENUMERATION_STOPPED) { - snprintf(reason, reason_len ,"Enumeration running"); + snprintf(reason, reason_len, "enumeration running"); _enum_sem.give(); return false; } @@ -652,17 +651,17 @@ bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len) uint8_t num_present_escs = __builtin_popcount(_esc_present_bitmask); if (num_present_escs < num_expected_motors) { - snprintf(reason, reason_len, "Too few ESCs detected"); + snprintf(reason, reason_len, "too few ESCs detected (%u of %u)", (int)num_present_escs, (int)num_expected_motors); return false; } if (num_present_escs > num_expected_motors) { - snprintf(reason, reason_len, "Too many ESCs detected"); + snprintf(reason, reason_len, "too many ESCs detected (%u > %u)", (int)num_present_escs, (int)num_expected_motors); return false; } if (_esc_max_node_id != num_expected_motors) { - snprintf(reason, reason_len, "Wrong node IDs, run enumeration"); + snprintf(reason, reason_len, "wrong node IDs (%u!=%u), run enumeration", (int)_esc_max_node_id, (int)num_expected_motors); return false; }