diff --git a/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp b/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp index 967a8c0e37..c9ee5ead4f 100644 --- a/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp +++ b/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp @@ -135,13 +135,14 @@ void AP_CANTester_KDECAN::loop(void) break; } case UPDATE_NODE_ID_OBJ_ADDR: { - if (_esc_info[i].enum_timeout != 0 && _esc_info[i].enum_timeout >= AP_HAL::micros64()) { + if (_esc_info[i].enum_timeout_ms != 0 + && _esc_info[i].enum_timeout_ms >= AP_HAL::millis()) { _esc_info[i].node_id = esc_num; _max_node_id = MAX(_max_node_id, esc_num - 2 + 1); gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Set node ID %d for ESC %d\n", esc_num, i); } - _esc_info[i].enum_timeout = 0; + _esc_info[i].enum_timeout_ms = 0; res_frame.dlc = 1; memcpy(res_frame.data, &(_esc_info[i].node_id), 1); @@ -149,8 +150,8 @@ void AP_CANTester_KDECAN::loop(void) break; } case START_ENUM_OBJ_ADDR: { - _esc_info[i].enum_timeout = AP_HAL::micros64() + be16toh(*((be16_t*) &(recv_frame.data[0]))) * 1000; - gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Starting enumeration for ESC %d, timeout %" PRIu64 "\n", i, _esc_info[i].enum_timeout); + _esc_info[i].enum_timeout_ms = AP_HAL::millis() + be16toh(*((be16_t*) &(recv_frame.data[0]))); + gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Starting enumeration for ESC %d, timeout %u", i, (unsigned)_esc_info[i].enum_timeout_ms); i++; continue; } @@ -202,15 +203,16 @@ void AP_CANTester_KDECAN::print_stats(void) if (counters[i].frame_id == 0) { break; } - hal.console->printf("0x%08" PRIX32 ": %" PRIu32 "\n", counters[i].frame_id, counters[i].count); + hal.console->printf("0x%08x: %u\n", (unsigned)counters[i].frame_id, (unsigned)counters[i].count); counters[i].count = 0; } } bool AP_CANTester_KDECAN::send_enumeration(uint8_t num) { - if (_esc_info[num].enum_timeout == 0 || AP_HAL::micros64() > _esc_info[num].enum_timeout) { - _esc_info[num].enum_timeout = 0; + if (_esc_info[num].enum_timeout_ms == 0 || + AP_HAL::millis() > _esc_info[num].enum_timeout_ms) { + _esc_info[num].enum_timeout_ms = 0; gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Not running enumeration for ESC %d\n", num); return false; } diff --git a/libraries/AP_CANManager/AP_CANTester_KDECAN.h b/libraries/AP_CANManager/AP_CANTester_KDECAN.h index 7470613f13..0bc2d0740c 100644 --- a/libraries/AP_CANManager/AP_CANTester_KDECAN.h +++ b/libraries/AP_CANManager/AP_CANTester_KDECAN.h @@ -49,9 +49,9 @@ private: struct esc_info { uint8_t node_id; uint64_t mcu_id; - uint64_t enum_timeout; + uint32_t enum_timeout_ms; - esc_info() : node_id(1), mcu_id(0), enum_timeout(0) {} + esc_info() : node_id(1), mcu_id(0), enum_timeout_ms(0) {} } _esc_info[NUM_ESCS]; uint8_t _max_node_id = 0;