AP_UAVCAN: add support for optionally publishing CANFD frames

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -103,7 +103,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: UAVCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0),
     
@@ -271,6 +271,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         }
         _node->setHardwareVersion(hw_version);
     }
+
+#if UAVCAN_SUPPORT_CANFD
+    if (option_is_set(Options::CANFD_ENABLED)) {
+        _node->enableCanFd();
+    }
+#endif
+
     int start_res = _node->start();
     if (start_res < 0) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res);
@@ -369,7 +376,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 
     snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index);
 
-    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, UAVCAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
         _node->setModeOfflineAndPublish();
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r");
         return;

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -34,12 +34,26 @@
 
 
 #ifndef UAVCAN_NODE_POOL_SIZE
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_NODE_POOL_SIZE 16384
+#else
 #define UAVCAN_NODE_POOL_SIZE 8192
 #endif
+#endif
+
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_STACK_SIZE     8192
+#else
+#define UAVCAN_STACK_SIZE     4096
+#endif
 
 #ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
+#else
 #define UAVCAN_NODE_POOL_BLOCK_SIZE 64
 #endif
+#endif
 
 #ifndef UAVCAN_SRV_NUMBER
 #define UAVCAN_SRV_NUMBER 18
@@ -116,7 +130,7 @@ public:
 
     void init(uint8_t driver_index, bool enable_filters) override;
     bool add_interface(AP_HAL::CANIface* can_iface) override;
-    
+
     uavcan::Node<0>* get_node() { return _node; }
     uint8_t get_driver_index() const { return _driver_index; }
 
@@ -206,6 +220,7 @@ public:
     enum class Options : uint16_t {
         DNA_CLEAR_DATABASE        = (1U<<0),
         DNA_IGNORE_DUPLICATE_NODE = (1U<<1),
+        CANFD_ENABLED             = (1U<<2),
     };
 
     // check if a option is set

libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp

@@ -48,7 +48,7 @@ int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFl
     if (can_iface_ == UAVCAN_NULLPTR) {
         return -1;
     }
-    return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, frame.dlc), tx_deadline.toUSec(), flags);
+    return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags);
 }
 
 /**
@@ -83,7 +83,7 @@ int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic,
     if (ret < 0) {
         return ret;
     }
-    out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, frame.dlc);
+    out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd);
     out_flags = flags;
     if (rx_timestamp != 0) {
         out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp);
@@ -185,7 +185,7 @@ CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const
                 msk.write |= (write ? 1 : 0) << i;
             }
         } else {
-            AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, pending_tx[i]->dlc};
+            AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)};
             if (iface->can_iface_->select(read, write, &frame, 0)) {
                 msk.read  |= (read ? 1 : 0) << i;
                 msk.write |= (write ? 1 : 0) << i;