diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp index bf3272519c..521489abbf 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.cpp +++ b/libraries/AP_KDECAN/AP_KDECAN.cpp @@ -235,14 +235,14 @@ void AP_KDECAN_Driver::loop() for (uint8_t i=0; i= TELEMETRY_INTERVAL_MS) { - if (send_packet(TELEMETRY_OBJ_ADDR, BROADCAST_NODE_ID, 10)) { + if (send_packet(TELEMETRY_OBJ_ADDR, BROADCAST_NODE_ID, 10000)) { _telemetry.timer_ms = now_ms; } } @@ -256,7 +256,7 @@ void AP_KDECAN_Driver::loop() broadcast_esc_info_next_interval_ms = 1000; } - if (send_packet(ESC_INFO_OBJ_ADDR, BROADCAST_NODE_ID, 100)) { + if (send_packet(ESC_INFO_OBJ_ADDR, BROADCAST_NODE_ID, 100000)) { _init.detected_bitmask_ms = now_ms; } } @@ -264,13 +264,13 @@ void AP_KDECAN_Driver::loop() } // while true } -bool AP_KDECAN_Driver::send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint16_t data) +bool AP_KDECAN_Driver::send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint16_t data) { const uint16_t data_be16 = htobe16(data); - return send_packet(address, dest_id, timeout_ms, (uint8_t*)&data_be16, 2); + return send_packet(address, dest_id, timeout_us, (uint8_t*)&data_be16, 2); } -bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint8_t *data, const uint8_t data_len) +bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint8_t *data, const uint8_t data_len) { // broadcast telemetry request frame const frame_id_t id { @@ -285,7 +285,6 @@ bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id, AP_HAL::CANFrame frame = AP_HAL::CANFrame((id.value | AP_HAL::CANFrame::FlagEFF), data, data_len, false); - const uint64_t timeout_us = uint64_t(timeout_ms) * 1000UL; return write_frame(frame, timeout_us); } diff --git a/libraries/AP_KDECAN/AP_KDECAN.h b/libraries/AP_KDECAN/AP_KDECAN.h index 635b100bbd..70ba23d984 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.h +++ b/libraries/AP_KDECAN/AP_KDECAN.h @@ -56,8 +56,8 @@ private: // handler for incoming frames void handle_frame(AP_HAL::CANFrame &frame) override; - bool send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint16_t data); - bool send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint8_t *data = nullptr, const uint8_t data_len = 0); + bool send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint16_t data); + bool send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint8_t *data = nullptr, const uint8_t data_len = 0); void loop();