diff --git a/autonomy/interfacing/can/README.md b/autonomy/interfacing/can/README.md index 9bc0874a..7a2d5db7 100644 --- a/autonomy/interfacing/can/README.md +++ b/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. diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index db95fb65..ebf0c056 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/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 diff --git a/autonomy/interfacing/can/include/can_core.hpp b/autonomy/interfacing/can/include/can_core.hpp index 2de6128b..03c7d02f 100644 --- a/autonomy/interfacing/can/include/can_core.hpp +++ b/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 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 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 { diff --git a/autonomy/interfacing/can/src/can_core.cpp b/autonomy/interfacing/can/src/can_core.cpp index d4cb4780..e859ea2e 100644 --- a/autonomy/interfacing/can/src/can_core.cpp +++ b/autonomy/interfacing/can/src/can_core.cpp @@ -1,5 +1,6 @@ #include "can_core.hpp" -#include // For std::system to call the external script for slcand +#include // For std::min (CAN-FD frame length clamping) +#include // For std::system to call the external script for slcand #include #include // For fcntl #include // 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(message.data.size(), static_cast(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(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(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(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()); diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index d00a1525..3249478c 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -37,19 +37,32 @@ 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; @@ -57,6 +70,8 @@ CanNode::CanNode() : Node("can_node"), can_core(this->get_logger()) { config.device_path = device_path; config.bustype = bustype; config.bitrate = bitrate; + config.data_bitrate = static_cast(data_bitrate); + config.enable_fd = enable_can_fd; config.receive_timeout_ms = 10000; // Initialize the CAN interface