From 6320599404f3212e40302bc844ad7b16056bc083 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 19 Feb 2022 09:35:51 +1100
Subject: [PATCH] AP_CANManager: support mavcan with CANFD_FRAME

---
 libraries/AP_CANManager/AP_CANManager.cpp | 75 +++++++++++++++--------
 1 file changed, 49 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
index 3ac6f087c4..589c7fdcda 100644
--- a/libraries/AP_CANManager/AP_CANManager.cpp
+++ b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -426,14 +426,29 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
  */
 void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
 {
-    mavlink_can_frame_t p;
-    mavlink_msg_can_frame_decode(&msg, &p);
-    if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
-        return;
-    }
     const uint16_t timeout_us = 2000;
-    AP_HAL::CANFrame frame{p.id, p.data, p.len};
-    hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_CAN_FRAME: {
+        mavlink_can_frame_t p;
+        mavlink_msg_can_frame_decode(&msg, &p);
+        if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
+            return;
+        }
+        AP_HAL::CANFrame frame{p.id, p.data, p.len};
+        hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+        break;
+    }
+    case MAVLINK_MSG_ID_CANFD_FRAME: {
+        mavlink_canfd_frame_t p;
+        mavlink_msg_canfd_frame_decode(&msg, &p);
+        if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
+            return;
+        }
+        AP_HAL::CANFrame frame{p.id, p.data, p.len, true};
+        hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+        break;
+    }
+    }
 }
 
 /*
@@ -527,7 +542,7 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg)
 
 /*
   handler for CAN frames from the registered callback, sending frames
-  out as CAN_FRAME messages
+  out as CAN_FRAME or CANFD_FRAME messages
  */
 void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
 {
@@ -543,26 +558,34 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
         can_forward.frame_counter = 0;
     }
     WITH_SEMAPHORE(comm_chan_lock(can_forward.chan));
-    if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
-        if (can_forward.num_filter_ids != 0) {
-            // work out ID of this frame
-            uint16_t id = 0;
-            if ((frame.id&0xff) != 0) {
-                // not anonymous
-                if (frame.id & 0x80) {
-                    // service message
-                    id = uint8_t(frame.id>>16);
-                } else {
-                    // message frame
-                    id = uint16_t(frame.id>>8);
-                }
-            }
-            if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) {
-                return;
+    if (can_forward.filter_ids != nullptr) {
+        // work out ID of this frame
+        uint16_t id = 0;
+        if ((frame.id&0xff) != 0) {
+            // not anonymous
+            if (frame.id & 0x80) {
+                // service message
+                id = uint8_t(frame.id>>16);
+            } else {
+                // message frame
+                id = uint16_t(frame.id>>8);
             }
         }
-        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));
+        if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) {
+            return;
+        }
+    }
+    const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
+    if (frame.isCanFDFrame()) {
+        if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) {
+            mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
+                                         bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
+        }
+    } else {
+        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, data_len, frame.id, const_cast<uint8_t*>(frame.data));
+        }
     }
 }
 #endif // HAL_GCS_ENABLED