From 6e83633a129bb6a49054c1d1d8a2b2f677a6bb16 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 14 Feb 2022 09:08:42 +1100
Subject: [PATCH] AP_CANManager: moved CAN_FRAME timeout code to CANManager

---
 libraries/AP_CANManager/AP_CANManager.cpp | 13 +++++++++++++
 libraries/AP_CANManager/AP_CANManager.h   |  4 +++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
index a4492c4c6c..bd2b63abd0 100644
--- a/libraries/AP_CANManager/AP_CANManager.cpp
+++ b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -387,6 +387,7 @@ void AP_CANManager::log_retrieve(ExpandingString &str) const
  */
 bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_command_long_t &packet, const mavlink_message_t &msg)
 {
+    WITH_SEMAPHORE(can_forward.sem);
     const int8_t bus = int8_t(packet.param1)-1;
     if (bus == -1) {
         for (auto can_iface : hal.can) {
@@ -403,6 +404,7 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
             FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &))) {
         return false;
     }
+    can_forward.last_callback_enable_ms = AP_HAL::millis();
     can_forward.chan = chan;
     can_forward.system_id = msg.sysid;
     can_forward.component_id = msg.compid;
@@ -430,6 +432,17 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
  */
 void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
 {
+    WITH_SEMAPHORE(can_forward.sem);
+    if (can_forward.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() - can_forward.last_callback_enable_ms > 5000) {
+            hal.can[bus]->register_frame_callback(nullptr);
+            return;
+        }
+        can_forward.frame_counter = 0;
+    }
     if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
         mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
                                    bus, frame.dlc, frame.id, const_cast<uint8_t*>(frame.data));
diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h
index f847af1007..4938f90208 100644
--- a/libraries/AP_CANManager/AP_CANManager.h
+++ b/libraries/AP_CANManager/AP_CANManager.h
@@ -25,7 +25,6 @@
 #include "AP_SLCANIface.h"
 #include "AP_CANDriver.h"
 #include <GCS_MAVLink/GCS.h>
-//#include <GCS_MAVLink/GCS_MAVLink.h>
 
 class AP_CANManager
 {
@@ -180,6 +179,9 @@ private:
         mavlink_channel_t chan;
         uint8_t system_id;
         uint8_t component_id;
+        uint8_t frame_counter;
+        uint32_t last_callback_enable_ms;
+        HAL_Semaphore sem;
     } can_forward;
 #endif // HAL_GCS_ENABLED
 };