From d9f79f42c88218086d1be16313c2b46c7d48c420 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Feb 2022 09:04:54 +1100 Subject: [PATCH] AP_HAL: moved CAN_FRAME timeout handling to AP_CANManager --- libraries/AP_HAL/CANIface.cpp | 11 ----------- libraries/AP_HAL/CANIface.h | 2 -- 2 files changed, 13 deletions(-) diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp index 9cf1d8a24c..5219ab74fe 100644 --- a/libraries/AP_HAL/CANIface.cpp +++ b/libraries/AP_HAL/CANIface.cpp @@ -71,16 +71,6 @@ int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanI { if (frame_callback) { if ((flags & IsMAVCAN) == 0) { - if (frame_counter++ == 100) { - // check every 100 frames for disabling CAN_FRAME send - // we stop sending after 5s if the client stops - // sending MAV_CMD_CAN_FORWARD requests - if (AP_HAL::millis() - last_callback_enable_ms > 5000) { - frame_callback = nullptr; - return 1; - } - frame_counter = 0; - } frame_callback(get_iface_num(), frame); } else { CanRxItem rx_item; @@ -98,7 +88,6 @@ int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanI */ bool AP_HAL::CANIface::register_frame_callback(FrameCb cb) { - last_callback_enable_ms = AP_HAL::millis(); frame_callback = cb; return true; } diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 0d11ea1159..68988ad0e0 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -225,8 +225,6 @@ protected: virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0; FrameCb frame_callback; - uint8_t frame_counter; - uint32_t last_callback_enable_ms; uint32_t bitrate_; OperatingMode mode_; };