Add opt-in CAN-FD support to can_core/can_node (untested on hardware)

autonomy/interfacing/can/README.md

@@ -53,3 +53,21 @@ Live joint mirror, mjlab sim parity, and interactive calibration are done — se
 `config/params.yaml` defaults: `can_interface=can0` `device_path=/dev/canable` `bustype=slcan` `bitrate=1000000`
 
 DBC: `autonomy/interfacing/dbc/humanoid.dbc` · Debug: `candump can0`
+
+## CAN-FD (opt-in, unvalidated on real hardware)
+
+`enable_can_fd` / `data_bitrate` in `params.yaml` exist as scaffolding for a future move to
+CAN-FD (higher data-phase bitrate, more bus headroom at high motor counts/control rates).
+Defaults are `enable_can_fd: false`, which preserves today's classic-CAN/SLCAN behavior
+exactly.
+
+**Do not flip this on against the real arm without bench-testing first.** Enabling it
+requires:
+- A CAN-FD-capable adapter (the CANable + SLCAN setup this repo documents cannot carry FD
+  frames — SLCAN is the Lawicel ASCII protocol, which has no FD framing). You need a native
+  SocketCAN-FD interface (`bustype: "socketcan"`), e.g. via `gs_usb`/candleLight firmware.
+- Re-validating CAN watchdog/timeout behavior on that new adapter before trusting it near
+  motors — see the `real-hardware-safety` skill.
+
+`can_node` will refuse to start (not silently fall back to classic CAN) if `enable_can_fd:
+true` is set together with `bustype: "slcan"`, since that combination can't work.

autonomy/interfacing/can/config/params.yaml

@@ -6,7 +6,11 @@ can_node:
     device_path: "/dev/canable"        # Serial device path for SLCAN
     bustype: "slcan"                   # Bus type to create a CAN interface: "socketcan" or "slcan"
     bitrate: 1000000                   # CAN bitrate in bps (125000, 250000, 500000, 1000000)
-
+
+    # CAN-FD -- opt-in, unvalidated on real hardware. See can/README.md before enabling.
+    enable_can_fd: false                # requires bustype="socketcan" + an FD-capable adapter
+    data_bitrate: 5000000               # CAN-FD data-phase bitrate in bps, if enable_can_fd
+
     # Communication Settings
     publish_rate_hz: 50                # Rate for checking incoming CAN messages
     receive_timeout_ms: 10000          # Timeout for receiving CAN messages

autonomy/interfacing/can/include/can_core.hpp

@@ -10,21 +10,26 @@ struct CanMessage {
   CanMessage(int size) : data(size, 0), dlc(size) {}
   CanMessage(int id, int size) : id(id), data(size, 0), dlc(size) {}
 
-  uint32_t id;                  // CAN message ID
-  std::vector<uint8_t> data;    // Message data 8 bytes
-  uint8_t dlc;                  // Data Length Code
-  bool is_extended_id = false;  // Extended frame format flag
-  bool is_remote_frame = false; // Remote transmission request flag
-  uint64_t timestamp_us;        // Timestamp in microseconds
+  uint32_t id;
+  std::vector<uint8_t> data; // up to 8 bytes, or 64 if is_fd
+  uint8_t dlc;
+  bool is_extended_id = false;
+  bool is_remote_frame = false;
+  bool is_fd = false;            // requires CanConfig::enable_fd on the interface
+  bool fd_bitrate_switch = true; // CANFD_BRS; only meaningful when is_fd is true
+  uint64_t timestamp_us;
 };
 
 struct CanConfig {
-  std::string interface_name;  // CAN interface name (e.g., "can0")
-  std::string device_path;     // Device path for SLCAN (e.g., "/dev/ttyACM0")
-  std::string bustype;         // Bus type: "socketcan" or "slcan"
-  uint32_t bitrate;            // Bitrate in bps for arbitration phase
-  uint32_t data_bitrate;       // Data bitrate in bps for CAN-FD data phase
-  uint32_t receive_timeout_ms; // Receive timeout in milliseconds
+  std::string interface_name;
+  std::string device_path; // SLCAN serial device (e.g., "/dev/ttyACM0")
+  std::string bustype;     // "socketcan" or "slcan"
+  uint32_t bitrate;        // arbitration-phase bps
+  uint32_t data_bitrate;   // CAN-FD data-phase bps -- not yet wired into interface setup
+  uint32_t receive_timeout_ms;
+  // Not supported with bustype="slcan" (SLCAN can't carry FD frames -- setupSlcan() refuses).
+  // Unvalidated on real hardware; see real-hardware-safety skill before testing on the arm.
+  bool enable_fd = false;
 };
 
 class CanCore {

autonomy/interfacing/can/src/can_core.cpp

@@ -1,5 +1,6 @@
 #include "can_core.hpp"
-#include <cstdlib> // For std::system to call the external script for slcand
+#include <algorithm> // For std::min (CAN-FD frame length clamping)
+#include <cstdlib>   // For std::system to call the external script for slcand
 #include <cstring>
 #include <fcntl.h>         // For fcntl
 #include <linux/can.h>     // Definitions for can frames and CAN FD
@@ -77,30 +78,61 @@ bool CanCore::sendMessage(const CanMessage& message) {
   ssize_t bytes_written = -1;
   size_t expected_frame_size = 0;
 
-  // Classic CAN frame only
-  struct can_frame frame;
-  std::memset(&frame, 0, sizeof(frame));
-
-  frame.can_id = message.id;
-  if (message.is_extended_id) {
-    frame.can_id |= CAN_EFF_FLAG;
-  }
-  if (message.is_remote_frame) {
-    frame.can_id |= CAN_RTR_FLAG;
+  if (message.is_fd && !config_.enable_fd) {
+    RCLCPP_ERROR(logger_, "Refusing to send: message.is_fd=true but this CanCore was not "
+                          "initialized with enable_fd=true.");
+    return false;
   }
 
-  if (message.data.size() > CAN_MAX_DLEN) {
-    frame.can_dlc = CAN_MAX_DLEN;
+  if (message.is_fd) {
+    struct canfd_frame fd_frame;
+    std::memset(&fd_frame, 0, sizeof(fd_frame));
+
+    fd_frame.can_id = message.id;
+    if (message.is_extended_id) {
+      fd_frame.can_id |= CAN_EFF_FLAG;
+    }
+    // CAN-FD has no RTR concept.
+    if (message.is_remote_frame) {
+      RCLCPP_ERROR(logger_, "CAN-FD does not support remote frames (RTR); rejecting message.");
+      return false;
+    }
+
+    fd_frame.len = static_cast<__u8>(
+        std::min<size_t>(message.data.size(), static_cast<size_t>(CANFD_MAX_DLEN)));
+    if (message.fd_bitrate_switch) {
+      fd_frame.flags |= CANFD_BRS;
+    }
+    std::memcpy(fd_frame.data, message.data.data(), fd_frame.len);
+
+    expected_frame_size = sizeof(struct canfd_frame);
+    bytes_written = write(socket_fd_, &fd_frame, expected_frame_size);
   } else {
-    frame.can_dlc = static_cast<__u8>(message.data.size());
-  }
+    // Classic CAN frame -- up to 8 data bytes.
+    struct can_frame frame;
+    std::memset(&frame, 0, sizeof(frame));
 
-  if (!message.is_remote_frame) {
-    std::memcpy(frame.data, message.data.data(), frame.can_dlc);
-  }
+    frame.can_id = message.id;
+    if (message.is_extended_id) {
+      frame.can_id |= CAN_EFF_FLAG;
+    }
+    if (message.is_remote_frame) {
+      frame.can_id |= CAN_RTR_FLAG;
+    }
+
+    if (message.data.size() > CAN_MAX_DLEN) {
+      frame.can_dlc = CAN_MAX_DLEN;
+    } else {
+      frame.can_dlc = static_cast<__u8>(message.data.size());
+    }
+
+    if (!message.is_remote_frame) {
+      std::memcpy(frame.data, message.data.data(), frame.can_dlc);
+    }
 
-  expected_frame_size = sizeof(struct can_frame);
-  bytes_written = write(socket_fd_, &frame, expected_frame_size);
+    expected_frame_size = sizeof(struct can_frame);
+    bytes_written = write(socket_fd_, &frame, expected_frame_size);
+  }
 
   if (bytes_written < 0) {
     RCLCPP_ERROR(logger_, "Failed to write CAN frame to socket: %s", strerror(errno));
@@ -123,8 +155,12 @@ bool CanCore::receiveMessage(CanMessage& message) {
     return false;
   }
 
-  struct can_frame frame;
-  ssize_t bytes_read = read(socket_fd_, &frame, sizeof(struct can_frame));
+  // Frame type (classic vs FD) is determined below by the byte count read() returns.
+  union {
+    struct can_frame cc;
+    struct canfd_frame fd;
+  } frame;
+  ssize_t bytes_read = read(socket_fd_, &frame, sizeof(frame));
 
   if (bytes_read < 0) {
     if (errno == EAGAIN || errno == EWOULDBLOCK) {
@@ -144,29 +180,34 @@ bool CanCore::receiveMessage(CanMessage& message) {
     return false;
   }
 
-  if (static_cast<size_t>(bytes_read) < sizeof(struct can_frame)) {
-    RCLCPP_WARN(logger_, "Incomplete CAN frame received. Read %zd bytes, expected %zu bytes.",
-                bytes_read, sizeof(struct can_frame));
-    return false;
-  }
-
-  message.is_extended_id = (frame.can_id & CAN_EFF_FLAG) ? true : false;
-  message.is_remote_frame = (frame.can_id & CAN_RTR_FLAG) ? true : false;
-
-  if (message.is_extended_id) {
-    message.id = frame.can_id & CAN_EFF_MASK;
-  } else {
-    message.id = frame.can_id & CAN_SFF_MASK;
-  }
-
-  message.dlc = frame.can_dlc;
-  message.data.resize(frame.can_dlc);
-  if (!message.is_remote_frame) {
-    std::memcpy(message.data.data(), frame.data, frame.can_dlc);
+  if (static_cast<size_t>(bytes_read) == sizeof(struct canfd_frame)) {
+    message.is_fd = true;
+    message.is_extended_id = (frame.fd.can_id & CAN_EFF_FLAG) ? true : false;
+    message.is_remote_frame = false; // CAN-FD has no RTR concept
+    message.id = message.is_extended_id ? (frame.fd.can_id & CAN_EFF_MASK)
+                                        : (frame.fd.can_id & CAN_SFF_MASK);
+    message.dlc = frame.fd.len;
+    message.data.resize(frame.fd.len);
+    std::memcpy(message.data.data(), frame.fd.data, frame.fd.len);
+  } else if (static_cast<size_t>(bytes_read) == sizeof(struct can_frame)) {
+    message.is_fd = false;
+    message.is_extended_id = (frame.cc.can_id & CAN_EFF_FLAG) ? true : false;
+    message.is_remote_frame = (frame.cc.can_id & CAN_RTR_FLAG) ? true : false;
+    message.id = message.is_extended_id ? (frame.cc.can_id & CAN_EFF_MASK)
+                                        : (frame.cc.can_id & CAN_SFF_MASK);
+    message.dlc = frame.cc.can_dlc;
+    if (!message.is_remote_frame) {
+      message.data.resize(frame.cc.can_dlc);
+      std::memcpy(message.data.data(), frame.cc.data, frame.cc.can_dlc);
+    } else {
+      // For RTR frames, data field is irrelevant but dlc indicates requested data length
+      message.data.clear();
+    }
   } else {
-    // For RTR frames, data field is irrelevant but dlc indicates requested data
-    // length
-    message.data.clear();
+    RCLCPP_WARN(logger_,
+                "Received frame of unexpected size %zd bytes (neither classic %zu nor FD %zu).",
+                bytes_read, sizeof(struct can_frame), sizeof(struct canfd_frame));
+    return false;
   }
 
   // This is to log received message details for debugging
@@ -214,6 +255,21 @@ bool CanCore::setupSocketCan() {
     return false;
   }
 
+  // Must be set before bind() for CAN_RAW_FD_FRAMES to take effect.
+  if (config_.enable_fd) {
+    int enable_canfd = 1;
+    if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
+                   sizeof(enable_canfd)) < 0) {
+      RCLCPP_ERROR(logger_,
+                   "Failed to enable CAN_RAW_FD_FRAMES on socket: %s. Is this interface/driver "
+                   "actually CAN-FD capable?",
+                   strerror(errno));
+      close(socket_fd_);
+      socket_fd_ = -1;
+      return false;
+    }
+  }
+
   // Bind socket to the CAN interface
   struct sockaddr_can addr;
   addr.can_family = AF_CAN;
@@ -230,12 +286,19 @@ bool CanCore::setupSocketCan() {
   initialized_ = true;
   connected_ = true;
 
-  RCLCPP_INFO(logger_, "SocketCAN interface %s setup completed successfully (Classic CAN mode).",
-              config_.interface_name.c_str());
+  RCLCPP_INFO(logger_, "SocketCAN interface %s setup completed successfully (%s mode).",
+              config_.interface_name.c_str(), config_.enable_fd ? "CAN-FD" : "Classic CAN");
   return true;
 }
 
 bool CanCore::setupSlcan() {
+  if (config_.enable_fd) {
+    RCLCPP_ERROR(logger_, "enable_fd=true is not supported with bustype=slcan (SLCAN can't "
+                          "carry CAN-FD frames). Use bustype=socketcan with an FD-capable "
+                          "adapter instead.");
+    return false;
+  }
+
   RCLCPP_INFO(logger_, "Setting up SLCAN interface '%s' via external script.",
               config_.interface_name.c_str());
   RCLCPP_INFO(logger_, "  Device path for script: %s", config_.device_path.c_str());

autonomy/interfacing/can/src/can_node.cpp

@@ -37,26 +37,41 @@ CanNode::CanNode() : Node("can_node"), can_core(this->get_logger()) {
   this->declare_parameter("bitrate", 500000);
   this->declare_parameter("receive_poll_interval_ms", 10);
   this->declare_parameter("receive_timeout_ms", 10000);
+  this->declare_parameter("enable_can_fd", false);
+  this->declare_parameter("data_bitrate", 5000000);
 
   // Get parameter values
   std::string can_interface = this->get_parameter("can_interface").as_string();
   std::string device_path = this->get_parameter("device_path").as_string();
   std::string bustype = this->get_parameter("bustype").as_string();
   int bitrate = this->get_parameter("bitrate").as_int();
+  bool enable_can_fd = this->get_parameter("enable_can_fd").as_bool();
+  int data_bitrate = this->get_parameter("data_bitrate").as_int();
 
   int receive_poll_interval_ms = this->get_parameter("receive_poll_interval_ms").as_int();
 
   RCLCPP_INFO(this->get_logger(),
-              "Loaded parameters: interface=%s, bustype=%s, bitrate=%d, "
-              "poll_interval_ms=%d",
-              can_interface.c_str(), bustype.c_str(), bitrate, receive_poll_interval_ms);
+              "Loaded parameters: interface=%s, bustype=%s, bitrate=%d, enable_can_fd=%s, "
+              "data_bitrate=%d, poll_interval_ms=%d",
+              can_interface.c_str(), bustype.c_str(), bitrate, enable_can_fd ? "true" : "false",
+              data_bitrate, receive_poll_interval_ms);
+
+  if (enable_can_fd && bustype == "slcan") {
+    RCLCPP_ERROR(this->get_logger(),
+                 "enable_can_fd=true with bustype=slcan is not supported (see can_core.cpp "
+                 "setupSlcan()). Refusing to start rather than silently falling back to "
+                 "classic CAN.");
+    return;
+  }
 
   // Configure CanCore
   CanConfig config;
   config.interface_name = can_interface;
   config.device_path = device_path;
   config.bustype = bustype;
   config.bitrate = bitrate;
+  config.data_bitrate = static_cast<uint32_t>(data_bitrate);
+  config.enable_fd = enable_can_fd;
   config.receive_timeout_ms = 10000;
 
   // Initialize the CAN interface