From bfdea7a125444d35c3a22af2c5951bc9a4a71c96 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 18 Aug 2026 16:40:56 -0500 Subject: [PATCH 1/5] feat(canopen): lightweight CANopen client + DS402 drive helper Add a new `canopen` component: a slim, standards-based CANopen (CiA 301) client / master with a CiA 402 (DS402) drive-profile helper, suitable for driving devices like the Basicmicro MCP236/MCP266 motor controllers over CAN (e.g. via the espp/twai component). Architecture (modeled on odrive_native): * include/detail/canopen_core.hpp -- host-buildable pure C++20 wire core with no ESP dependencies: a transport-agnostic CanFrame struct (mirrors espp::Twai::Message field-for-field), NMT command / SYNC / PDO frame builders, heartbeat parsing, SDO request builders and response parser (expedited up/download with correct ccs/scs/e/s/n command specifiers, segmented upload with a small toggle-checking assembler, abort frames), an abort-code -> human-readable-string map, little-endian helpers, and the CiA 402 object indices, controlword commands, and statusword state decoding (standard 0x4F / 0x6F masks). * include/canopen_client.hpp -- espp::CanopenClient (BaseComponent): transport-agnostic via a user send function; the app feeds received frames to process_frame() (e.g. from the Twai on_receive callback). Blocking SDO transactions (condition variable + configurable timeout, one in flight per client, serialized by a mutex) with typed read_u8..read_i32 / write_u8..write_i32 accessors and read_string() (segmented upload, e.g. device name 0x1008). Non-blocking NMT master / SYNC / RPDO transmit helpers, per-COB-ID TPDO reception dispatch, and cached heartbeat NMT states with an optional callback. * include/ds402.hpp -- espp::Ds402Drive layered on a CanopenClient: statusword state decoding, enable_operation() walking Shutdown (0x0006) -> Switch On (0x0007) -> Enable Operation (0x000F) with statusword polling + timeout, disable(), quick_stop(), fault_reset() (rising edge of controlword bit 7), set_mode() verified via 0x6061 (profile velocity / profile position / homing), set_target_velocity(), and set_target_position() with the new-set-point handshake (controlword bits 4/5/6 + statusword bit-12 acknowledge), plus accessors for the standard identification, profile, and actual-value objects. * test/canopen_host_test.cpp -- golden-frame host unit tests (NMT, SYNC, PDO, heartbeat, SDO expedited up/download for u8/u16/u32, aborts, segmented upload of a 10-char string with toggle handling, LE helpers, and CiA 402 statusword decode vectors). Builds with `c++ -std=c++20 -I include test/canopen_host_test.cpp`. * example/ -- esp32 example wiring espp::Twai to the client: NMT start, SDO identity / device type / device name reads, then (for CiA 402 devices) profile-velocity enable + gentle ramp + stop. Also registers the component in the docs (Doxyfile, doc/en/buses) and CI (build matrix + component upload). Verified: host test passes, example builds for esp32 (IDF v6.0.1), and cppcheck is clean. Co-Authored-By: Claude Fable 5 --- .github/workflows/build.yml | 2 + .github/workflows/upload_components.yml | 1 + components/canopen/CMakeLists.txt | 7 + components/canopen/README.md | 47 ++ components/canopen/example/CMakeLists.txt | 22 + components/canopen/example/README.md | 68 +++ .../canopen/example/main/CMakeLists.txt | 2 + .../canopen/example/main/canopen_example.cpp | 173 ++++++ components/canopen/example/sdkconfig.defaults | 4 + components/canopen/idf_component.yml | 26 + components/canopen/include/canopen_client.hpp | 472 +++++++++++++++ .../canopen/include/detail/canopen_core.hpp | 547 ++++++++++++++++++ components/canopen/include/ds402.hpp | 360 ++++++++++++ components/canopen/test/canopen_host_test.cpp | 303 ++++++++++ doc/Doxyfile | 4 + doc/en/buses/canopen.rst | 45 ++ doc/en/buses/canopen_example.md | 2 + doc/en/buses/index.rst | 1 + 18 files changed, 2086 insertions(+) create mode 100644 components/canopen/CMakeLists.txt create mode 100644 components/canopen/README.md create mode 100644 components/canopen/example/CMakeLists.txt create mode 100644 components/canopen/example/README.md create mode 100644 components/canopen/example/main/CMakeLists.txt create mode 100644 components/canopen/example/main/canopen_example.cpp create mode 100644 components/canopen/example/sdkconfig.defaults create mode 100644 components/canopen/idf_component.yml create mode 100644 components/canopen/include/canopen_client.hpp create mode 100644 components/canopen/include/detail/canopen_core.hpp create mode 100644 components/canopen/include/ds402.hpp create mode 100644 components/canopen/test/canopen_host_test.cpp create mode 100644 doc/en/buses/canopen.rst create mode 100644 doc/en/buses/canopen_example.md diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index f95d008263..0b15e26375 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -86,6 +86,8 @@ jobs: target: esp32 - path: 'components/byte90/example' target: esp32s3 + - path: 'components/canopen/example' + target: esp32 - path: 'components/chsc6x/example' target: esp32s3 - path: 'components/cdr/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 5fe19fbcc9..fd6098d442 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -61,6 +61,7 @@ jobs: components/bmi270 components/button components/byte90 + components/canopen components/chsc6x components/cdr components/cli diff --git a/components/canopen/CMakeLists.txt b/components/canopen/CMakeLists.txt new file mode 100644 index 0000000000..a722e8a4d8 --- /dev/null +++ b/components/canopen/CMakeLists.txt @@ -0,0 +1,7 @@ +# NOTE: like odrive_native, this component's detail/ lives INSIDE include/ +# (include/detail/canopen_core.hpp), so registering "include" alone makes +# `#include "detail/canopen_core.hpp"` resolve for consumers. +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES base_component +) diff --git a/components/canopen/README.md b/components/canopen/README.md new file mode 100644 index 0000000000..b66abc2232 --- /dev/null +++ b/components/canopen/README.md @@ -0,0 +1,47 @@ +# CANopen (CiA 301) Client Component + +[![Badge](https://components.espressif.com/components/espp/canopen/badge.svg)](https://components.espressif.com/components/espp/canopen) + +The `CanopenClient` class provides a lightweight, standards-based CANopen +(CiA 301) client / master for talking to a CANopen server node - for example a +Basicmicro MCP236/MCP266 motor controller - over a classic CAN 2.0 bus. The +`Ds402Drive` class layers the CiA 402 (DS402) drive profile on top of it: +statusword state-machine decoding, the enable-operation sequence, fault reset, +mode selection, and profile-velocity / profile-position motion helpers. + +Implemented CiA 301 services (deliberately slim): + +* **NMT master** commands (COB-ID `0x000`): start / stop / pre-operational / + reset node / reset communication, addressed to one node or all nodes. +* **Heartbeat / boot-up consumption** (COB-ID `0x700` + node id): the NMT state + of every producing node is cached (with an optional callback). +* **SDO client** (COB-IDs `0x600`/`0x580` + node id): expedited upload + (read) and download (write) of 1/2/4-byte objects with typed + `read_u8..read_i32` / `write_u8..write_i32` wrappers, segmented upload for + strings (e.g. manufacturer device name `0x1008`) with toggle-bit handling, + and abort-code parsing with human-readable messages. +* **PDO helpers**: RPDO transmit (pack + send on a COB-ID) and TPDO reception + dispatch via per-COB-ID callbacks. +* **SYNC** (COB-ID `0x080`) transmission. + +The component is **transport-agnostic**: it transmits by invoking a +user-provided `send` function with a plain `espp::detail::CanFrame`, and the +application feeds received frames to `process_frame()`. The `CanFrame` struct +mirrors `espp::Twai::Message` field-for-field, so wiring it to the `espp/twai` +component is a two-line conversion (see the example) - but any CAN transport +(external SPI CAN controller, USB-CAN bridge, ...) works just as well. + +SDO transactions are blocking with a configurable timeout; `process_frame()` +must be called from a different task than the one performing SDO transfers +(automatic with `espp::Twai`, whose `on_receive` runs in its own task). + +The wire core (`include/detail/canopen_core.hpp`) is host-buildable pure C++20 +with no ESP dependencies, and is covered by golden-frame unit tests in +`test/canopen_host_test.cpp`. + +## Example + +The [example](./example) uses an `espp::Twai` transport to NMT-start a node, +read its identity and device type via SDO, and - if the device implements +CiA 402 - switch it to profile velocity mode, enable operation, run a gentle +velocity ramp, and stop. diff --git a/components/canopen/example/CMakeLists.txt b/components/canopen/example/CMakeLists.txt new file mode 100644 index 0000000000..ca86c2ad30 --- /dev/null +++ b/components/canopen/example/CMakeLists.txt @@ -0,0 +1,22 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +# add the component directories that we want to use +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py canopen twai" + CACHE STRING + "List of components to include" + ) + +project(canopen_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/canopen/example/README.md b/components/canopen/example/README.md new file mode 100644 index 0000000000..f18e1ba9e8 --- /dev/null +++ b/components/canopen/example/README.md @@ -0,0 +1,68 @@ +# CANopen Client Example + +This example demonstrates the use of the `espp::CanopenClient` and +`espp::Ds402Drive` classes to talk to a CANopen (CiA 301) server node - for +example a Basicmicro MCP236/MCP266 motor controller - over the ESP TWAI +(CAN 2.0) peripheral via the `espp::Twai` class. + +It: + +* Brings up the TWAI peripheral in `NORMAL` mode and wires its task-context + `on_receive` callback to `CanopenClient::process_frame()` (the + transport-agnostic `CanFrame` struct mirrors `espp::Twai::Message` + field-for-field). +* Sends an **NMT start** to a configurable node id. +* **SDO-reads** the standard identification objects: device type (`0x1000`), + identity (`0x1018` vendor / product / revision / serial), and the + manufacturer device name (`0x1008`, a string read via **segmented** SDO + upload with toggle-bit handling). +* If the device reports the CiA 402 device profile: selects **profile velocity + mode**, walks the CiA 402 state machine to **Operation Enabled** (with an + automatic fault reset if needed), runs a gentle velocity ramp up and back + down while logging the actual velocity, then stops and disables the drive. + +## How to use example + +### Hardware Required + +An ESP chip with a TWAI peripheral (e.g. ESP32 or ESP32-S3), a 3.3V CAN +transceiver (e.g. SN65HVD230, TJA1050, MCP2551) wired between the configured +TX/RX GPIOs and the bus, and a CANopen device on a properly (120Ω) terminated +bus. Update the `node_id`, GPIOs, and baudrate at the top of the example to +match your setup (Basicmicro MCP2xx controllers default to 250 kbit/s). + +``` + ESP32 GPIO(tx) ---> CTX \ + SN65HVD230 ==> CANH / CANL (120R terminated bus) + ESP32 GPIO(rx) <--- CRX / +``` + +### Build and Flash + +``` +idf.py set-target esp32 +idf.py -p PORT flash monitor +``` + +(To exit the serial monitor, type ``Ctrl-]``.) + +## Example Output + +``` +[CANopen Example/I][0.518]: Starting CANopen (CiA 301) client example! +[CANopen Example/I][0.530]: Sent NMT start to node 1 +[CANopen Example/I][0.735]: Device type (0x1000): 0x00020192 +[CANopen Example/I][0.740]: Vendor id (0x1018:1): 0x00000123 +[CANopen Example/I][0.746]: Product code (0x1018:2): 0x00000266 +[CANopen Example/I][0.752]: Revision (0x1018:3): 0x00010000 +[CANopen Example/I][0.758]: Serial number (0x1018:4): 0x0000BEEF +[CANopen Example/I][0.770]: Device name (0x1008): 'MCP266 2x60A' +[CANopen Example/I][0.776]: Drive state: Switch on disabled +[Ds402Drive/I][0.850]: enable_operation: starting from state 'Switch on disabled' +[Ds402Drive/I][1.050]: enable_operation: drive is in Operation Enabled +[CANopen Example/I][1.560]: target= 100, actual= 98 +... +[CANopen Example/I][6.560]: target= 0, actual= 1 +[CANopen Example/I][6.660]: Motion demo complete +[CANopen Example/I][6.665]: CANopen example complete! +``` diff --git a/components/canopen/example/main/CMakeLists.txt b/components/canopen/example/main/CMakeLists.txt new file mode 100644 index 0000000000..a941e22ba7 --- /dev/null +++ b/components/canopen/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/canopen/example/main/canopen_example.cpp b/components/canopen/example/main/canopen_example.cpp new file mode 100644 index 0000000000..87bf0a15af --- /dev/null +++ b/components/canopen/example/main/canopen_example.cpp @@ -0,0 +1,173 @@ +#include +#include + +#include "canopen_client.hpp" +#include "ds402.hpp" +#include "twai.hpp" + +using namespace std::chrono_literals; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "CANopen Example", .level = espp::Logger::Verbosity::INFO}); + logger.info("Starting CANopen (CiA 301) client example!"); + + //! [canopen example] + // The CANopen node id of the device we want to talk to (e.g. a Basicmicro + // MCP236/MCP266 motor controller). Change to match your device. + static constexpr uint8_t node_id = 1; + + // Forward-declared handle so the Twai on_receive callback (registered at + // Twai construction) can feed frames to the client we construct just below. + static espp::CanopenClient *client_ptr = nullptr; + + // Bring up the TWAI (CAN 2.0) peripheral. NOTE: talking to a real CANopen + // device requires Mode::NORMAL with a 3.3V CAN transceiver (e.g. SN65HVD230) + // wired to the tx/rx GPIOs, a properly terminated bus, and a matching + // baudrate (Basicmicro MCP2xx default is 250 kbit/s). + // + // The on_receive callback runs in the Twai receive task, i.e. NOT in the + // task performing the (blocking) SDO transactions below -- which is exactly + // what CanopenClient::process_frame() requires. The CanFrame struct mirrors + // espp::Twai::Message field-for-field, so conversion is trivial. + espp::Twai twai({ + .tx_gpio = 5, // GPIO5 (change to match your board / transceiver) + .rx_gpio = 4, // GPIO4 (change to match your board / transceiver) + .baudrate = 250000, + .mode = espp::Twai::Mode::NORMAL, + .tx_queue_depth = 5, + .on_receive = + [](const espp::Twai::Message &msg) { + if (client_ptr) { + client_ptr->process_frame(espp::CanopenClient::CanFrame{ + .id = msg.id, + .extended = msg.extended, + .rtr = msg.rtr, + .dlc = msg.dlc, + .data = msg.data, + }); + } + }, + .log_level = espp::Logger::Verbosity::INFO, + }); + + // The CANopen client is transport-agnostic: give it a send function which + // transmits an espp::detail::CanFrame (here: over TWAI). The CanFrame struct + // mirrors espp::Twai::Message field-for-field, so conversion is trivial. + espp::CanopenClient client({ + .node_id = node_id, + .send = + [&twai](const espp::CanopenClient::CanFrame &frame) { + espp::Twai::Message msg{ + .id = frame.id, + .extended = frame.extended, + .rtr = frame.rtr, + .dlc = frame.dlc, + .data = frame.data, + }; + std::error_code tx_ec; + return twai.transmit(msg, tx_ec); + }, + .sdo_timeout = 100ms, + .on_heartbeat = + [&logger](uint8_t hb_node, espp::CanopenClient::NmtState state) { + logger.info("Heartbeat from node {}: NMT state {}", hb_node, static_cast(state)); + }, + .log_level = espp::Logger::Verbosity::INFO, + }); + client_ptr = &client; + + std::error_code ec; + if (!twai.initialize(ec)) { + logger.error("Failed to initialize TWAI: {}", ec.message()); + return; + } + + // NMT: put the node into Operational so its PDOs (if any) are active. + if (!client.nmt_start(ec)) { + logger.error("Failed to send NMT start: {}", ec.message()); + return; + } + logger.info("Sent NMT start to node {}", node_id); + std::this_thread::sleep_for(100ms); + + // SDO: read the standard identification objects. + espp::Ds402Drive drive( + client, + {.state_timeout = 1s, .poll_period = 20ms, .log_level = espp::Logger::Verbosity::INFO}); + + auto device_type = drive.get_device_type(ec); + if (ec) { + logger.error("Failed to read device type (0x1000): {} -- is the node on the bus?", + ec.message()); + return; + } + logger.info("Device type (0x1000): 0x{:08X}", device_type); + // device profile number is in the lower 16 bits; 402 => a CiA 402 drive + const bool is_ds402 = (device_type & 0xFFFF) == 402; + + logger.info("Vendor id (0x1018:1): 0x{:08X}", drive.get_vendor_id(ec)); + logger.info("Product code (0x1018:2): 0x{:08X}", drive.get_product_code(ec)); + logger.info("Revision (0x1018:3): 0x{:08X}", drive.get_revision_number(ec)); + logger.info("Serial number (0x1018:4): 0x{:08X}", drive.get_serial_number(ec)); + // manufacturer device name (0x1008) is a string -> segmented SDO upload + auto name = drive.get_device_name(ec); + if (!ec) { + logger.info("Device name (0x1008): '{}'", name); + } + + if (!is_ds402) { + logger.warn("Device does not report the CiA 402 profile; skipping motion demo"); + } else { + // DS402: profile velocity mode, enable, gentle ramp, stop, disable. + if (auto state = drive.get_state(ec); !ec) { + logger.info("Drive state: {}", espp::detail::ds402::state_to_string(state)); + if (state == espp::Ds402Drive::State::Fault) { + logger.info("Drive is in Fault; attempting fault reset"); + if (!drive.fault_reset(ec)) { + logger.error("Fault reset failed: {}", ec.message()); + return; + } + } + } + + if (!drive.set_mode(espp::Ds402Drive::OperatingMode::ProfileVelocity, ec)) { + logger.error("Failed to set profile velocity mode: {}", ec.message()); + return; + } + // conservative profile accel / decel (device units) + drive.set_profile_acceleration(1000, ec); + drive.set_profile_deceleration(1000, ec); + + if (!drive.enable_operation(ec)) { + logger.error("Failed to enable operation: {}", ec.message()); + return; + } + + // gentle velocity ramp up and back down + static constexpr int32_t max_velocity = 500; // device units, keep it gentle + static constexpr int32_t step = 100; + for (int32_t v = step; v <= max_velocity; v += step) { + drive.set_target_velocity(v, ec); + std::this_thread::sleep_for(500ms); + logger.info("target={:4}, actual={:4}", v, drive.get_velocity_actual(ec)); + } + for (int32_t v = max_velocity - step; v >= 0; v -= step) { + drive.set_target_velocity(v, ec); + std::this_thread::sleep_for(500ms); + logger.info("target={:4}, actual={:4}", v, drive.get_velocity_actual(ec)); + } + + // stop and disable the power stage + drive.set_target_velocity(0, ec); + if (!drive.disable(ec)) { + logger.error("Failed to disable drive: {}", ec.message()); + } + logger.info("Motion demo complete"); + } + //! [canopen example] + + logger.info("CANopen example complete!"); + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/canopen/example/sdkconfig.defaults b/components/canopen/example/sdkconfig.defaults new file mode 100644 index 0000000000..c3667f3e33 --- /dev/null +++ b/components/canopen/example/sdkconfig.defaults @@ -0,0 +1,4 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 diff --git a/components/canopen/idf_component.yml b/components/canopen/idf_component.yml new file mode 100644 index 0000000000..09d88f52e6 --- /dev/null +++ b/components/canopen/idf_component.yml @@ -0,0 +1,26 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Lightweight CANopen (CiA 301) client / master with a DS402 (CiA 402) drive-profile helper" +url: "https://github.com/esp-cpp/espp/tree/main/components/canopen" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/buses/canopen.html" +examples: + - path: example +tags: + - cpp + - Component + - CANopen + - CAN + - DS402 + - CiA301 + - CiA402 + - NMT + - SDO + - PDO + - Motor +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' diff --git a/components/canopen/include/canopen_client.hpp b/components/canopen/include/canopen_client.hpp new file mode 100644 index 0000000000..46cdda553c --- /dev/null +++ b/components/canopen/include/canopen_client.hpp @@ -0,0 +1,472 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "detail/canopen_core.hpp" + +namespace espp { + +/// \brief A lightweight CANopen (CiA 301) client / master for a single server node. +/// \details Implements the master-side services needed to drive a typical +/// CANopen device (e.g. a Basicmicro MCP236/MCP266 motor controller): +/// - NMT master commands (start / stop / pre-operational / reset) +/// - Heartbeat & boot-up consumption (cached state + optional callback) +/// - SDO client: expedited upload/download of 1/2/4-byte objects, and +/// segmented upload (for strings such as device name 0x1008) +/// - PDO helpers: RPDO transmit and per-COB-ID TPDO reception dispatch +/// - SYNC transmission +/// +/// The client is transport-agnostic: it transmits by calling the +/// configured \c send function with a espp::detail::CanFrame, and the +/// application feeds every received frame to process_frame() (e.g. +/// from the espp::Twai \c on_receive callback -- the frame layouts +/// match field-for-field). +/// +/// SDO transactions are blocking: the calling task sends the request +/// and waits on a condition variable until process_frame() delivers +/// the matching response (COB-ID 0x580 + node id) or the configured +/// timeout expires. \b Note: process_frame() must therefore be called +/// from a different task than the one performing SDO reads/writes; +/// with espp::Twai this is automatically the case since \c on_receive +/// runs in the Twai receive task. One SDO transaction may be in +/// flight per client at a time (serialized internally by a mutex); +/// NMT / SYNC / PDO helpers are non-blocking and unserialized. +/// +/// \section canopen_ex0 CANopen Client Example +/// \snippet canopen_example.cpp canopen example +class CanopenClient : public BaseComponent { +public: + using CanFrame = detail::CanFrame; ///< Transport-agnostic CAN frame type. + using NmtCommand = detail::canopen::NmtCommand; ///< NMT master command specifier. + using NmtState = detail::canopen::NmtState; ///< NMT state (heartbeat / boot-up). + + /// \brief Function used to transmit a frame on the bus. + /// \details Should return true if the frame was (queued to be) sent. + typedef std::function send_fn; + + /// \brief Callback invoked (from the process_frame() context) for every + /// heartbeat / boot-up frame received from any node. + typedef std::function heartbeat_callback_fn; + + /// \brief Callback invoked (from the process_frame() context) for a received + /// frame on a registered TPDO COB-ID. + typedef std::function pdo_callback_fn; + + /// \brief Configuration for the CanopenClient. + struct Config { + uint8_t node_id; ///< Server node id (1-127) this client talks to. + send_fn send; ///< Function used to transmit frames on the bus. + std::chrono::milliseconds sdo_timeout{100}; ///< Timeout for one SDO round-trip. + heartbeat_callback_fn on_heartbeat{nullptr}; ///< Optional heartbeat / boot-up callback. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Logger verbosity. + }; + + /// \brief Create a CANopen client. + /// \param config The configuration. + explicit CanopenClient(const Config &config) + : BaseComponent("CanopenClient", config.log_level) + , node_id_(config.node_id) + , send_(config.send) + , sdo_timeout_(config.sdo_timeout) + , on_heartbeat_(config.on_heartbeat) {} + + /// \brief The configured server node id. + uint8_t node_id() const { return node_id_; } + + /// \brief Feed a received CAN frame to the client. + /// \details Call this for every frame received from the bus (e.g. from the + /// espp::Twai \c on_receive callback). Dispatches SDO responses to + /// the waiting transaction, caches heartbeat states (invoking the + /// optional heartbeat callback), and dispatches registered TPDO + /// callbacks. Must not be called from the task performing SDO + /// transactions (see class description). + /// \param frame The received frame. + void process_frame(const CanFrame &frame) { + // SDO response from our server node? + if (frame.id == detail::canopen::COB_SDO_TX_BASE + node_id_) { + { + std::lock_guard lock(response_mutex_); + if (awaiting_response_) { + response_ = detail::canopen::parse_sdo_response(frame); + awaiting_response_ = false; + } + } + response_cv_.notify_all(); + return; + } + // heartbeat / boot-up from any node? + uint8_t hb_node = 0; + if (auto state = detail::canopen::parse_heartbeat(frame, hb_node); state.has_value()) { + logger_.debug("Heartbeat from node {}: state 0x{:02X}", hb_node, + static_cast(frame.data[0])); + { + std::lock_guard lock(state_mutex_); + node_states_[hb_node] = *state; + } + if (on_heartbeat_) { + on_heartbeat_(hb_node, *state); + } + return; + } + // registered TPDO? + pdo_callback_fn callback{nullptr}; + { + std::lock_guard lock(pdo_mutex_); + if (auto it = pdo_callbacks_.find(frame.id); it != pdo_callbacks_.end()) { + callback = it->second; + } + } + if (callback) { + callback(frame); + } + } + + /// @name NMT master / SYNC / PDO (non-blocking) + /// @{ + + /// \brief Send an NMT master command. + /// \param command The command specifier. + /// \param target_node_id Target node id, or 0 to address all nodes. + /// \param ec Set on transmit failure. + /// \return True on success. + bool send_nmt(NmtCommand command, uint8_t target_node_id, std::error_code &ec) { + return send_frame(detail::canopen::make_nmt(command, target_node_id), ec); + } + + /// \brief NMT-start the configured server node. \param ec Set on failure. \return True on + /// success. + bool nmt_start(std::error_code &ec) { return send_nmt(NmtCommand::Start, node_id_, ec); } + /// \brief NMT-stop the configured server node. \param ec Set on failure. \return True on success. + bool nmt_stop(std::error_code &ec) { return send_nmt(NmtCommand::Stop, node_id_, ec); } + /// \brief Put the configured server node into pre-operational. \param ec Set on failure. + /// \return True on success. + bool nmt_pre_operational(std::error_code &ec) { + return send_nmt(NmtCommand::PreOperational, node_id_, ec); + } + /// \brief Reset the configured server node (application reset). \param ec Set on failure. + /// \return True on success. + bool nmt_reset_node(std::error_code &ec) { return send_nmt(NmtCommand::ResetNode, node_id_, ec); } + /// \brief Reset communication of the configured server node. \param ec Set on failure. + /// \return True on success. + bool nmt_reset_communication(std::error_code &ec) { + return send_nmt(NmtCommand::ResetCommunication, node_id_, ec); + } + + /// \brief Send a SYNC frame (COB-ID 0x080). + /// \param ec Set on transmit failure. + /// \return True on success. + bool send_sync(std::error_code &ec) { return send_frame(detail::canopen::make_sync(), ec); } + + /// \brief Transmit an RPDO (build + send a data frame on \p cob_id). + /// \param cob_id COB-ID to transmit on (e.g. 0x200 + node id for RPDO1). + /// \param data Packed application data (up to 8 bytes). + /// \param ec Set on transmit failure. + /// \return True on success. + bool send_rpdo(uint32_t cob_id, std::span data, std::error_code &ec) { + return send_frame(detail::canopen::make_pdo(cob_id, data), ec); + } + + /// \brief Register a callback for received frames on a TPDO COB-ID. + /// \param cob_id COB-ID to match (e.g. 0x180 + node id for TPDO1). + /// \param callback Invoked from the process_frame() context; replaces any + /// previous callback for this COB-ID. + void register_tpdo_callback(uint32_t cob_id, pdo_callback_fn callback) { + std::lock_guard lock(pdo_mutex_); + pdo_callbacks_[cob_id] = callback; + } + + /// \brief Remove the callback registered for a TPDO COB-ID. + /// \param cob_id The COB-ID whose callback should be removed. + void unregister_tpdo_callback(uint32_t cob_id) { + std::lock_guard lock(pdo_mutex_); + pdo_callbacks_.erase(cob_id); + } + + /// @} + + /// @name Heartbeat state + /// @{ + + /// \brief The last NMT state heard (via heartbeat / boot-up) from a node. + /// \param target_node_id The node id to query. + /// \return The last state, or std::nullopt if nothing was heard from that node. + std::optional get_nmt_state(uint8_t target_node_id) const { + std::lock_guard lock(state_mutex_); + if (auto it = node_states_.find(target_node_id); it != node_states_.end()) { + return it->second; + } + return std::nullopt; + } + + /// \brief The last NMT state heard from the configured server node. + /// \return The last state, or std::nullopt if nothing was heard yet. + std::optional get_nmt_state() const { return get_nmt_state(node_id_); } + + /// @} + + /// @name SDO client (blocking, expedited) + /// @{ + + /// \brief Write (SDO expedited download) raw little-endian object data. + /// \param index Object dictionary index. + /// \param subindex Object dictionary subindex. + /// \param data Object data, little-endian, 1, 2, or 4 bytes. + /// \param ec Set on transmit failure, timeout, or SDO abort. + /// \return True on success. + bool sdo_download(uint16_t index, uint8_t subindex, std::span data, + std::error_code &ec) { + std::lock_guard lock(sdo_mutex_); + detail::canopen::SdoResponse response; + if (!sdo_transact(detail::canopen::make_sdo_expedited_download(node_id_, index, subindex, data), + response, index, subindex, ec)) { + return false; + } + if (response.type != detail::canopen::SdoResponse::Type::DownloadOk) { + logger_.error("SDO download 0x{:04X}:{:02X}: unexpected response type", index, subindex); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + return true; + } + + /// \brief Read (SDO expedited upload) raw little-endian object data. + /// \param index Object dictionary index. + /// \param subindex Object dictionary subindex. + /// \param out Destination for the object data (little-endian). + /// \param ec Set on transmit failure, timeout, SDO abort, or if the object is + /// larger than \p out (use read_string() for segmented transfers). + /// \return Number of bytes read (> 0), or 0 on error. + size_t sdo_upload(uint16_t index, uint8_t subindex, std::span out, std::error_code &ec) { + std::lock_guard lock(sdo_mutex_); + detail::canopen::SdoResponse response; + if (!sdo_transact(detail::canopen::make_sdo_upload_request(node_id_, index, subindex), response, + index, subindex, ec)) { + return 0; + } + if (response.type != detail::canopen::SdoResponse::Type::ExpeditedUpload || + response.len > out.size()) { + logger_.error("SDO upload 0x{:04X}:{:02X}: not an expedited response of <= {} bytes", index, + subindex, out.size()); + ec = std::make_error_code(std::errc::protocol_error); + return 0; + } + std::copy_n(response.data.begin(), response.len, out.begin()); + return response.len; + } + + /// \brief Read a string object via SDO segmented (or expedited) upload. + /// \details Handles the toggle-bit protocol for multi-segment transfers; used + /// e.g. for the manufacturer device name (0x1008). + /// \param index Object dictionary index. + /// \param subindex Object dictionary subindex. + /// \param ec Set on transmit failure, timeout, SDO abort, or toggle error. + /// \return The string data, or an empty string on error. + std::string read_string(uint16_t index, uint8_t subindex, std::error_code &ec) { + std::lock_guard lock(sdo_mutex_); + detail::canopen::SdoResponse response; + if (!sdo_transact(detail::canopen::make_sdo_upload_request(node_id_, index, subindex), response, + index, subindex, ec)) { + return {}; + } + using Type = detail::canopen::SdoResponse::Type; + if (response.type == Type::ExpeditedUpload) { + return {reinterpret_cast(response.data.data()), response.len}; + } + if (response.type != Type::SegmentedUploadInit) { + logger_.error("SDO upload 0x{:04X}:{:02X}: unexpected response type", index, subindex); + ec = std::make_error_code(std::errc::protocol_error); + return {}; + } + detail::canopen::SdoSegmentedUpload assembler; + assembler.start(response); + while (!assembler.done()) { + if (!sdo_transact( + detail::canopen::make_sdo_upload_segment_request(node_id_, assembler.next_toggle()), + response, index, subindex, ec)) { + return {}; + } + if (response.type != Type::UploadSegment || !assembler.consume(response)) { + logger_.error("SDO segmented upload 0x{:04X}:{:02X}: bad segment (toggle mismatch?)", index, + subindex); + send_frame_quietly(detail::canopen::make_sdo_abort(node_id_, index, subindex, 0x05030000)); + ec = std::make_error_code(std::errc::protocol_error); + return {}; + } + } + // strings may be null-padded; trim at the first NUL + std::string data = assembler.data(); + if (auto pos = data.find('\0'); pos != std::string::npos) { + data.resize(pos); + } + return data; + } + + /// \brief Write an unsigned 8-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_u8(uint16_t index, uint8_t subindex, uint8_t value, std::error_code &ec) { + return write_le(index, subindex, value, 1, ec); + } + /// \brief Write an unsigned 16-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_u16(uint16_t index, uint8_t subindex, uint16_t value, std::error_code &ec) { + return write_le(index, subindex, value, 2, ec); + } + /// \brief Write an unsigned 32-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_u32(uint16_t index, uint8_t subindex, uint32_t value, std::error_code &ec) { + return write_le(index, subindex, value, 4, ec); + } + /// \brief Write a signed 8-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_i8(uint16_t index, uint8_t subindex, int8_t value, std::error_code &ec) { + return write_le(index, subindex, static_cast(value), 1, ec); + } + /// \brief Write a signed 16-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_i16(uint16_t index, uint8_t subindex, int16_t value, std::error_code &ec) { + return write_le(index, subindex, static_cast(value), 2, ec); + } + /// \brief Write a signed 32-bit object. \param index Object index. \param subindex Object + /// subindex. \param value Value to write. \param ec Set on failure. \return True on success. + bool write_i32(uint16_t index, uint8_t subindex, int32_t value, std::error_code &ec) { + return write_le(index, subindex, static_cast(value), 4, ec); + } + + /// \brief Read an unsigned 8-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + uint8_t read_u8(uint16_t index, uint8_t subindex, std::error_code &ec) { + return static_cast(read_le(index, subindex, 1, ec)); + } + /// \brief Read an unsigned 16-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + uint16_t read_u16(uint16_t index, uint8_t subindex, std::error_code &ec) { + return static_cast(read_le(index, subindex, 2, ec)); + } + /// \brief Read an unsigned 32-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + uint32_t read_u32(uint16_t index, uint8_t subindex, std::error_code &ec) { + return read_le(index, subindex, 4, ec); + } + /// \brief Read a signed 8-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + int8_t read_i8(uint16_t index, uint8_t subindex, std::error_code &ec) { + return static_cast(read_le(index, subindex, 1, ec)); + } + /// \brief Read a signed 16-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + int16_t read_i16(uint16_t index, uint8_t subindex, std::error_code &ec) { + return static_cast(read_le(index, subindex, 2, ec)); + } + /// \brief Read a signed 32-bit object. \param index Object index. \param subindex Object + /// subindex. \param ec Set on failure. \return The value (0 on error). + int32_t read_i32(uint16_t index, uint8_t subindex, std::error_code &ec) { + return static_cast(read_le(index, subindex, 4, ec)); + } + + /// \brief The abort code from the most recent SDO abort response (0 if none). + uint32_t last_abort_code() const { + std::lock_guard lock(response_mutex_); + return last_abort_code_; + } + + /// @} + +protected: + bool send_frame(const CanFrame &frame, std::error_code &ec) { + if (!send_ || !send_(frame)) { + logger_.error("Failed to send frame with id 0x{:03X}", frame.id); + ec = std::make_error_code(std::errc::io_error); + return false; + } + return true; + } + + void send_frame_quietly(const CanFrame &frame) { + if (send_) { + send_(frame); + } + } + + /// Perform one SDO request/response round-trip. Must be called with + /// sdo_mutex_ held. On an abort response, logs the decoded abort reason and + /// fails with protocol_error. + bool sdo_transact(const CanFrame &request, detail::canopen::SdoResponse &response, uint16_t index, + uint8_t subindex, std::error_code &ec) { + { + std::lock_guard lock(response_mutex_); + awaiting_response_ = true; + } + if (!send_frame(request, ec)) { + std::lock_guard lock(response_mutex_); + awaiting_response_ = false; + return false; + } + std::unique_lock lock(response_mutex_); + if (!response_cv_.wait_for(lock, sdo_timeout_, [this] { return !awaiting_response_; })) { + awaiting_response_ = false; + logger_.error("SDO 0x{:04X}:{:02X}: timed out after {} ms waiting for response from node {}", + index, subindex, sdo_timeout_.count(), node_id_); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + response = response_; + if (response.type == detail::canopen::SdoResponse::Type::Abort) { + last_abort_code_ = response.abort_code; + lock.unlock(); + logger_.error("SDO 0x{:04X}:{:02X}: aborted with code 0x{:08X} ({})", index, subindex, + response.abort_code, detail::canopen::sdo_abort_to_string(response.abort_code)); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + return true; + } + + bool write_le(uint16_t index, uint8_t subindex, uint32_t value, size_t num_bytes, + std::error_code &ec) { + std::array data{}; + detail::canopen::put_le(value, data.data(), num_bytes); + return sdo_download(index, subindex, std::span(data.data(), num_bytes), ec); + } + + uint32_t read_le(uint16_t index, uint8_t subindex, size_t num_bytes, std::error_code &ec) { + std::array data{}; + const auto len = sdo_upload(index, subindex, std::span(data.data(), num_bytes), ec); + if (len == 0) { + return 0; + } + return detail::canopen::get_le(data.data(), len); + } + + uint8_t node_id_; + send_fn send_; + std::chrono::milliseconds sdo_timeout_; + heartbeat_callback_fn on_heartbeat_; + + // SDO transaction state: sdo_mutex_ serializes whole transactions; + // response_mutex_ / response_cv_ hand the parsed response from + // process_frame() to the waiting transaction. + std::mutex sdo_mutex_; + mutable std::mutex response_mutex_; + std::condition_variable response_cv_; + bool awaiting_response_{false}; + detail::canopen::SdoResponse response_{}; + uint32_t last_abort_code_{0}; + + mutable std::mutex state_mutex_; + std::unordered_map node_states_; + + std::mutex pdo_mutex_; + std::unordered_map pdo_callbacks_; +}; + +} // namespace espp diff --git a/components/canopen/include/detail/canopen_core.hpp b/components/canopen/include/detail/canopen_core.hpp new file mode 100644 index 0000000000..f815e7e207 --- /dev/null +++ b/components/canopen/include/detail/canopen_core.hpp @@ -0,0 +1,547 @@ +#pragma once + +// Host-buildable CANopen (CiA 301) wire core: frame builders / parsers with no +// ESP-IDF (or even espp) dependencies -- pure C++20 standard library. All +// multi-byte object data is little-endian per CiA 301. The espp::CanopenClient +// component layers transport wiring, blocking SDO transactions, and logging on +// top of this core; the host unit tests exercise this file directly. + +#include +#include +#include +#include +#include +#include + +namespace espp::detail { + +/// \brief A transport-agnostic classic CAN 2.0 frame. +/// \details Deliberately mirrors espp::Twai::Message field-for-field so the two +/// convert trivially, without this component depending on any CAN +/// driver. +struct CanFrame { + uint32_t id{0}; ///< Arbitration ID (11-bit standard, or 29-bit if \c extended). + bool extended{false}; ///< True for an extended (29-bit) ID. CANopen uses standard IDs. + bool rtr{false}; ///< True for a Remote Transmission Request frame. + uint8_t dlc{0}; ///< Number of valid data bytes (0-8). + std::array data{}; ///< Frame payload (only the first \c dlc bytes valid). +}; + +namespace canopen { + +/// @name Well-known COB-ID bases (pre-defined connection set, CiA 301 §7.3.5) +/// @{ +inline constexpr uint32_t COB_NMT = 0x000; ///< NMT master command (broadcast). +inline constexpr uint32_t COB_SYNC = 0x080; ///< SYNC producer. +inline constexpr uint32_t COB_EMCY_BASE = 0x080; ///< EMCY: 0x080 + node id. +inline constexpr uint32_t COB_TPDO1_BASE = 0x180; ///< TPDO1: 0x180 + node id. +inline constexpr uint32_t COB_RPDO1_BASE = 0x200; ///< RPDO1: 0x200 + node id. +inline constexpr uint32_t COB_TPDO2_BASE = 0x280; ///< TPDO2: 0x280 + node id. +inline constexpr uint32_t COB_RPDO2_BASE = 0x300; ///< RPDO2: 0x300 + node id. +inline constexpr uint32_t COB_TPDO3_BASE = 0x380; ///< TPDO3: 0x380 + node id. +inline constexpr uint32_t COB_RPDO3_BASE = 0x400; ///< RPDO3: 0x400 + node id. +inline constexpr uint32_t COB_TPDO4_BASE = 0x480; ///< TPDO4: 0x480 + node id. +inline constexpr uint32_t COB_RPDO4_BASE = 0x500; ///< RPDO4: 0x500 + node id. +inline constexpr uint32_t COB_SDO_TX_BASE = 0x580; ///< SDO server->client (response). +inline constexpr uint32_t COB_SDO_RX_BASE = 0x600; ///< SDO client->server (request). +inline constexpr uint32_t COB_HEARTBEAT_BASE = 0x700; ///< Heartbeat / boot-up: 0x700 + node id. +/// @} + +/// \brief NMT master command specifiers (CiA 301 §7.2.8.3.1). +enum class NmtCommand : uint8_t { + Start = 0x01, ///< Start remote node (-> Operational). + Stop = 0x02, ///< Stop remote node (-> Stopped). + PreOperational = 0x80, ///< Enter pre-operational. + ResetNode = 0x81, ///< Reset node (application reset). + ResetCommunication = 0x82, ///< Reset communication. +}; + +/// \brief NMT states as reported in heartbeat / boot-up frames (CiA 301 §7.2.8.3.2.2). +enum class NmtState : uint8_t { + BootUp = 0x00, ///< Boot-up message (device just initialized). + Stopped = 0x04, ///< Stopped. + Operational = 0x05, ///< Operational. + PreOperational = 0x7F, ///< Pre-operational. + Unknown = 0xFF, ///< Not a standard state (or nothing heard yet). +}; + +/// \brief Store an unsigned value little-endian into \p out (LSB first). +/// \param value Value to store. +/// \param out Destination bytes; \p num_bytes are written. +/// \param num_bytes Number of bytes to write (1..4). +inline void put_le(uint32_t value, uint8_t *out, size_t num_bytes) { + std::generate_n(out, num_bytes, [value, shift = 0u]() mutable { + const auto b = static_cast((value >> shift) & 0xFF); + shift += 8; + return b; + }); +} + +/// \brief Load a little-endian unsigned value from \p in. +/// \param in Source bytes. +/// \param num_bytes Number of bytes to read (1..4). +/// \return The decoded value. +inline uint32_t get_le(const uint8_t *in, size_t num_bytes) { + uint32_t value = 0; + for (size_t i = 0; i < num_bytes; ++i) { + value |= static_cast(in[i]) << (8 * i); + } + return value; +} + +/// \brief Build an NMT master command frame (COB-ID 0x000). +/// \param command The NMT command specifier. +/// \param node_id Target node id (1-127), or 0 to address all nodes. +/// \return The frame to transmit. +inline CanFrame make_nmt(NmtCommand command, uint8_t node_id) { + CanFrame f; + f.id = COB_NMT; + f.dlc = 2; + f.data[0] = static_cast(command); + f.data[1] = node_id; + return f; +} + +/// \brief Build a SYNC frame (COB-ID 0x080, no data). +/// \return The frame to transmit. +inline CanFrame make_sync() { + CanFrame f; + f.id = COB_SYNC; + f.dlc = 0; + return f; +} + +/// \brief Build an RPDO (or any raw data) frame for a given COB-ID. +/// \param cob_id The COB-ID to transmit on (e.g. 0x200 + node id for RPDO1). +/// \param data Packed application data (up to 8 bytes; extra bytes ignored). +/// \return The frame to transmit. +inline CanFrame make_pdo(uint32_t cob_id, std::span data) { + CanFrame f; + f.id = cob_id; + f.dlc = static_cast(std::min(data.size(), f.data.size())); + std::copy_n(data.begin(), f.dlc, f.data.begin()); + return f; +} + +/// \brief Parse a heartbeat / boot-up frame (COB-ID 0x700 + node id). +/// \param frame The received frame. +/// \param[out] node_id On success, the producing node id (1-127). +/// \return The reported NMT state, or std::nullopt if \p frame is not a heartbeat. +inline std::optional parse_heartbeat(const CanFrame &frame, uint8_t &node_id) { + if (frame.extended || frame.rtr || frame.dlc < 1) { + return std::nullopt; + } + if (frame.id <= COB_HEARTBEAT_BASE || frame.id > COB_HEARTBEAT_BASE + 0x7F) { + return std::nullopt; + } + node_id = static_cast(frame.id - COB_HEARTBEAT_BASE); + const uint8_t state = frame.data[0] & 0x7F; // mask the (historic) toggle bit + switch (state) { + case 0x00: + return NmtState::BootUp; + case 0x04: + return NmtState::Stopped; + case 0x05: + return NmtState::Operational; + case 0x7F: + return NmtState::PreOperational; + default: + return NmtState::Unknown; + } +} + +/// @name SDO (Service Data Object) protocol, expedited + segmented upload (CiA 301 §7.2.4) +/// @{ + +/// \brief Build an expedited SDO download (write) initiate request. +/// \details Byte 0 is the command: ccs=1 (initiate download), e=1 (expedited), +/// s=1 (size indicated), n = 4 - \p data.size() empty bytes; i.e. 0x2F +/// for 1 byte, 0x2B for 2 bytes, 0x23 for 4 bytes. +/// \param node_id Server node id (request goes to COB-ID 0x600 + node id). +/// \param index Object dictionary index. +/// \param subindex Object dictionary subindex. +/// \param data Object data, little-endian, 1, 2, or 4 bytes. +/// \return The frame to transmit. +inline CanFrame make_sdo_expedited_download(uint8_t node_id, uint16_t index, uint8_t subindex, + std::span data) { + CanFrame f; + f.id = COB_SDO_RX_BASE + node_id; + f.dlc = 8; + const auto len = std::min(data.size(), 4); + const auto n = static_cast(4 - len); + f.data[0] = static_cast(0x20 | (n << 2) | 0x02 | 0x01); // ccs=1, e=1, s=1 + put_le(index, &f.data[1], 2); + f.data[3] = subindex; + std::copy_n(data.begin(), len, f.data.begin() + 4); + return f; +} + +/// \brief Build an SDO upload (read) initiate request (ccs=2, byte 0 = 0x40). +/// \param node_id Server node id. +/// \param index Object dictionary index. +/// \param subindex Object dictionary subindex. +/// \return The frame to transmit. +inline CanFrame make_sdo_upload_request(uint8_t node_id, uint16_t index, uint8_t subindex) { + CanFrame f; + f.id = COB_SDO_RX_BASE + node_id; + f.dlc = 8; + f.data[0] = 0x40; // ccs=2 (initiate upload) + put_le(index, &f.data[1], 2); + f.data[3] = subindex; + return f; +} + +/// \brief Build an SDO upload segment request (ccs=3, byte 0 = 0x60 | toggle<<4). +/// \param node_id Server node id. +/// \param toggle Toggle bit; must alternate starting at false for the first segment. +/// \return The frame to transmit. +inline CanFrame make_sdo_upload_segment_request(uint8_t node_id, bool toggle) { + CanFrame f; + f.id = COB_SDO_RX_BASE + node_id; + f.dlc = 8; + f.data[0] = static_cast(0x60 | (toggle ? 0x10 : 0x00)); + return f; +} + +/// \brief Build an SDO abort transfer frame (cs=0x80). +/// \param node_id Server node id (the abort is sent on the request COB-ID). +/// \param index Object dictionary index the abort refers to. +/// \param subindex Object dictionary subindex the abort refers to. +/// \param abort_code CiA 301 abort code. +/// \return The frame to transmit. +inline CanFrame make_sdo_abort(uint8_t node_id, uint16_t index, uint8_t subindex, + uint32_t abort_code) { + CanFrame f; + f.id = COB_SDO_RX_BASE + node_id; + f.dlc = 8; + f.data[0] = 0x80; + put_le(index, &f.data[1], 2); + f.data[3] = subindex; + put_le(abort_code, &f.data[4], 4); + return f; +} + +/// \brief A parsed SDO server response (COB-ID 0x580 + node id). +struct SdoResponse { + /// \brief The kind of response, from the server command specifier (scs). + enum class Type : uint8_t { + DownloadOk, ///< scs=3: download initiate confirmed. + ExpeditedUpload, ///< scs=2, e=1: expedited upload data in \c data. + SegmentedUploadInit, ///< scs=2, e=0: segmented upload; \c total_size may be indicated. + UploadSegment, ///< scs=0: upload segment data in \c data. + Abort, ///< cs=0x80: transfer aborted; see \c abort_code. + Unknown, ///< Unrecognized command specifier. + }; + Type type{Type::Unknown}; + uint16_t index{0}; ///< Object index (valid for initiate / abort responses). + uint8_t subindex{0}; ///< Object subindex (valid for initiate / abort responses). + std::array data{}; ///< Payload bytes (expedited: up to 4; segment: up to 7). + uint8_t len{0}; ///< Number of valid bytes in \c data. + bool size_indicated{false}; ///< True if the server indicated a size. + uint32_t total_size{0}; ///< Total transfer size (SegmentedUploadInit with size_indicated). + bool toggle{false}; ///< Toggle bit (UploadSegment). + bool last{false}; ///< True if this UploadSegment is the final one (c bit). + uint32_t abort_code{0}; ///< Abort code (Abort). +}; + +/// \brief Parse an SDO server response frame. +/// \param frame A frame received on COB-ID 0x580 + node id (caller checks the id). +/// \return The parsed response; type is Type::Unknown if the command specifier +/// is unrecognized or the frame is malformed. +inline SdoResponse parse_sdo_response(const CanFrame &frame) { + SdoResponse r; + if (frame.dlc < 1) { + return r; + } + const uint8_t cmd = frame.data[0]; + const uint8_t scs = cmd >> 5; + if (cmd == 0x80) { + // abort transfer + r.type = SdoResponse::Type::Abort; + r.index = static_cast(get_le(&frame.data[1], 2)); + r.subindex = frame.data[3]; + r.abort_code = get_le(&frame.data[4], 4); + return r; + } + switch (scs) { + case 3: // initiate download response + r.type = SdoResponse::Type::DownloadOk; + r.index = static_cast(get_le(&frame.data[1], 2)); + r.subindex = frame.data[3]; + return r; + case 2: { // initiate upload response + r.index = static_cast(get_le(&frame.data[1], 2)); + r.subindex = frame.data[3]; + const bool expedited = (cmd & 0x02) != 0; + r.size_indicated = (cmd & 0x01) != 0; + if (expedited) { + r.type = SdoResponse::Type::ExpeditedUpload; + const auto n = static_cast((cmd >> 2) & 0x03); + r.len = r.size_indicated ? static_cast(4 - n) : 4; + std::copy_n(frame.data.begin() + 4, r.len, r.data.begin()); + } else { + r.type = SdoResponse::Type::SegmentedUploadInit; + if (r.size_indicated) { + r.total_size = get_le(&frame.data[4], 4); + } + } + return r; + } + case 0: { // upload segment response + r.type = SdoResponse::Type::UploadSegment; + r.toggle = (cmd & 0x10) != 0; + r.last = (cmd & 0x01) != 0; + const auto n = static_cast((cmd >> 1) & 0x07); // bytes NOT containing data + r.len = static_cast(7 - n); + std::copy_n(frame.data.begin() + 1, r.len, r.data.begin()); + return r; + } + default: + return r; + } +} + +/// \brief Small accumulator for a segmented SDO upload. +/// \details Feed the parsed SegmentedUploadInit response, then each parsed +/// UploadSegment response; verifies the toggle-bit alternation and +/// collects the payload bytes. The caller drives the request side +/// (make_sdo_upload_segment_request with next_toggle()). +class SdoSegmentedUpload { +public: + /// \brief Start a transfer from a parsed SegmentedUploadInit response. + /// \param init The parsed initiate response. + void start(const SdoResponse &init) { + data_.clear(); + toggle_ = false; + done_ = false; + if (init.size_indicated) { + data_.reserve(init.total_size); + } + } + + /// \brief The toggle bit to use for the next segment request. + bool next_toggle() const { return toggle_; } + + /// \brief Consume a parsed UploadSegment response. + /// \param segment The parsed segment response. + /// \return False on toggle-bit mismatch (protocol error), true otherwise. + bool consume(const SdoResponse &segment) { + if (segment.toggle != toggle_) { + return false; + } + data_.append(reinterpret_cast(segment.data.data()), segment.len); + toggle_ = !toggle_; + done_ = segment.last; + return true; + } + + /// \brief True once the final segment (c bit) has been consumed. + bool done() const { return done_; } + + /// \brief The accumulated payload bytes. + const std::string &data() const { return data_; } + +private: + std::string data_{}; + bool toggle_{false}; + bool done_{false}; +}; + +/// \brief Map a CiA 301 SDO abort code to a human-readable string. +/// \param abort_code The 32-bit abort code from an SDO abort frame. +/// \return A static description string ("unknown abort code" if unmapped). +inline const char *sdo_abort_to_string(uint32_t abort_code) { + switch (abort_code) { + case 0x05030000: + return "toggle bit not alternated"; + case 0x05040000: + return "SDO protocol timed out"; + case 0x05040001: + return "invalid or unknown command specifier"; + case 0x05040002: + return "invalid block size"; + case 0x05040003: + return "invalid sequence number"; + case 0x05040004: + return "CRC error"; + case 0x05040005: + return "out of memory"; + case 0x06010000: + return "unsupported access to object"; + case 0x06010001: + return "attempt to read a write-only object"; + case 0x06010002: + return "attempt to write a read-only object"; + case 0x06020000: + return "object does not exist in the object dictionary"; + case 0x06040041: + return "object cannot be mapped to the PDO"; + case 0x06040042: + return "number and length of mapped objects would exceed PDO length"; + case 0x06040043: + return "general parameter incompatibility"; + case 0x06040047: + return "general internal incompatibility in the device"; + case 0x06060000: + return "access failed due to a hardware error"; + case 0x06070010: + return "data type does not match, length of service parameter does not match"; + case 0x06070012: + return "data type does not match, length of service parameter too high"; + case 0x06070013: + return "data type does not match, length of service parameter too low"; + case 0x06090011: + return "subindex does not exist"; + case 0x06090030: + return "invalid value for parameter"; + case 0x06090031: + return "value of parameter written too high"; + case 0x06090032: + return "value of parameter written too low"; + case 0x06090036: + return "maximum value is less than minimum value"; + case 0x08000000: + return "general error"; + case 0x08000020: + return "data cannot be transferred or stored to the application"; + case 0x08000021: + return "data cannot be transferred or stored because of local control"; + case 0x08000022: + return "data cannot be transferred or stored because of the present device state"; + case 0x08000023: + return "object dictionary dynamic generation fails or no object dictionary present"; + default: + return "unknown abort code"; + } +} + +/// @} + +} // namespace canopen + +namespace ds402 { + +/// @name Standard CiA 402 / device object dictionary indices +/// @{ +inline constexpr uint16_t OBJ_DEVICE_TYPE = 0x1000; ///< Device type (u32). +inline constexpr uint16_t OBJ_ERROR_REGISTER = 0x1001; ///< Error register (u8). +inline constexpr uint16_t OBJ_DEVICE_NAME = 0x1008; ///< Manufacturer device name (string). +inline constexpr uint16_t OBJ_IDENTITY = 0x1018; ///< Identity object (subs 1-4). +inline constexpr uint16_t OBJ_CONTROLWORD = 0x6040; ///< Controlword (u16). +inline constexpr uint16_t OBJ_STATUSWORD = 0x6041; ///< Statusword (u16). +inline constexpr uint16_t OBJ_MODES_OF_OPERATION = 0x6060; ///< Modes of operation (i8). +inline constexpr uint16_t OBJ_MODES_OF_OPERATION_DISPLAY = 0x6061; ///< Modes display (i8). +inline constexpr uint16_t OBJ_POSITION_ACTUAL = 0x6064; ///< Position actual value (i32). +inline constexpr uint16_t OBJ_VELOCITY_ACTUAL = 0x606C; ///< Velocity actual value (i32). +inline constexpr uint16_t OBJ_TARGET_POSITION = 0x607A; ///< Target position (i32). +inline constexpr uint16_t OBJ_PROFILE_VELOCITY = 0x6081; ///< Profile velocity (u32). +inline constexpr uint16_t OBJ_PROFILE_ACCELERATION = 0x6083; ///< Profile acceleration (u32). +inline constexpr uint16_t OBJ_PROFILE_DECELERATION = 0x6084; ///< Profile deceleration (u32). +inline constexpr uint16_t OBJ_TARGET_VELOCITY = 0x60FF; ///< Target velocity (i32). +/// @} + +/// @name Controlword command values (CiA 402 §8.2.1) +/// @{ +inline constexpr uint16_t CW_SHUTDOWN = 0x0006; ///< Shutdown -> Ready to switch on. +inline constexpr uint16_t CW_SWITCH_ON = 0x0007; ///< Switch on -> Switched on. +inline constexpr uint16_t CW_ENABLE_OPERATION = 0x000F; ///< Enable operation. +inline constexpr uint16_t CW_DISABLE_VOLTAGE = 0x0000; ///< Disable voltage -> Switch on disabled. +inline constexpr uint16_t CW_QUICK_STOP = 0x0002; ///< Quick stop. +inline constexpr uint16_t CW_FAULT_RESET = 0x0080; ///< Fault reset (rising edge of bit 7). +inline constexpr uint16_t CW_BIT_NEW_SETPOINT = + 0x0010; ///< Bit 4: new set-point (profile position). +inline constexpr uint16_t CW_BIT_CHANGE_SET_IMMEDIATELY = + 0x0020; ///< Bit 5: change set immediately. +inline constexpr uint16_t CW_BIT_RELATIVE = 0x0040; ///< Bit 6: target position is relative. +/// @} + +/// @name Statusword bits (CiA 402 §8.2.2) +/// @{ +inline constexpr uint16_t SW_BIT_TARGET_REACHED = 0x0400; ///< Bit 10: target reached. +inline constexpr uint16_t SW_BIT_SETPOINT_ACKNOWLEDGE = 0x1000; ///< Bit 12 (profile position). +/// @} + +/// \brief CiA 402 power drive system state, decoded from the statusword. +enum class State : uint8_t { + NotReadyToSwitchOn, ///< xxxx xxxx x0xx 0000 + SwitchOnDisabled, ///< xxxx xxxx x1xx 0000 + ReadyToSwitchOn, ///< xxxx xxxx x01x 0001 + SwitchedOn, ///< xxxx xxxx x01x 0011 + OperationEnabled, ///< xxxx xxxx x01x 0111 + QuickStopActive, ///< xxxx xxxx x00x 0111 + FaultReactionActive, ///< xxxx xxxx x0xx 1111 + Fault, ///< xxxx xxxx x0xx 1000 + Unknown, ///< Statusword did not match any standard state pattern. +}; + +/// \brief Standard CiA 402 modes of operation (object 0x6060 / 0x6061). +enum class OperatingMode : int8_t { + ProfilePosition = 1, ///< Profile position mode (pp). + ProfileVelocity = 3, ///< Profile velocity mode (pv). + ProfileTorque = 4, ///< Profile torque mode (tq). + Homing = 6, ///< Homing mode (hm). +}; + +/// \brief Decode a CiA 402 statusword into a drive state. +/// \details Applies the standard bit masks: mask 0x4F distinguishes +/// Not-ready / Switch-on-disabled / Fault / Fault-reaction; mask 0x6F +/// distinguishes Ready-to-switch-on / Switched-on / Operation-enabled / +/// Quick-stop-active. +/// \param statusword The raw statusword (object 0x6041). +/// \return The decoded state. +inline State decode_state(uint16_t statusword) { + switch (statusword & 0x4F) { + case 0x00: + return State::NotReadyToSwitchOn; + case 0x40: + return State::SwitchOnDisabled; + case 0x08: + return State::Fault; + case 0x0F: + return State::FaultReactionActive; + default: + break; + } + switch (statusword & 0x6F) { + case 0x21: + return State::ReadyToSwitchOn; + case 0x23: + return State::SwitchedOn; + case 0x27: + return State::OperationEnabled; + case 0x07: + return State::QuickStopActive; + default: + return State::Unknown; + } +} + +/// \brief Get a human-readable name for a CiA 402 drive state. +/// \param state The decoded state. +/// \return A static name string. +inline const char *state_to_string(State state) { + switch (state) { + case State::NotReadyToSwitchOn: + return "Not ready to switch on"; + case State::SwitchOnDisabled: + return "Switch on disabled"; + case State::ReadyToSwitchOn: + return "Ready to switch on"; + case State::SwitchedOn: + return "Switched on"; + case State::OperationEnabled: + return "Operation enabled"; + case State::QuickStopActive: + return "Quick stop active"; + case State::FaultReactionActive: + return "Fault reaction active"; + case State::Fault: + return "Fault"; + default: + return "Unknown"; + } +} + +} // namespace ds402 + +} // namespace espp::detail diff --git a/components/canopen/include/ds402.hpp b/components/canopen/include/ds402.hpp new file mode 100644 index 0000000000..ada2268100 --- /dev/null +++ b/components/canopen/include/ds402.hpp @@ -0,0 +1,360 @@ +#pragma once + +#include +#include +#include +#include + +#include "base_component.hpp" +#include "canopen_client.hpp" + +namespace espp { + +/// \brief CiA 402 (DS402) drive-profile helper, layered on a CanopenClient. +/// \details Wraps the standard CiA 402 objects (controlword 0x6040, statusword +/// 0x6041, modes of operation 0x6060/0x6061, target/actual velocity & +/// position, profile velocity/acceleration/deceleration) and the +/// power-drive-system state machine: decode the statusword into a +/// State, walk the enable sequence (Shutdown -> Switch On -> Enable +/// Operation) with statusword polling and timeout, quick-stop, and +/// fault reset (rising edge of controlword bit 7). Supports the +/// profile velocity (pv), profile position (pp), and homing (hm) +/// modes, including the new-set-point handshake (controlword bits +/// 4/5/6) for profile position moves. +/// +/// All communication goes through the referenced CanopenClient's SDO +/// channel, so the same threading rules apply (calls block, and +/// frames must be delivered to the client from another task). +/// +/// \section ds402_ex0 DS402 Drive Example +/// \snippet canopen_example.cpp canopen example +class Ds402Drive : public BaseComponent { +public: + using State = detail::ds402::State; ///< CiA 402 drive state. + using OperatingMode = detail::ds402::OperatingMode; ///< CiA 402 mode of operation. + + /// \brief Configuration for the Ds402Drive. + struct Config { + std::chrono::milliseconds state_timeout{ + 1000}; ///< Timeout for each state transition / mode change to take effect. + std::chrono::milliseconds poll_period{10}; ///< Statusword polling period. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Logger verbosity. + }; + + /// \brief Create a DS402 drive helper. + /// \param client The CANopen client for the drive's node. Must outlive this object. + /// \param config The configuration. + explicit Ds402Drive(CanopenClient &client, const Config &config) + : BaseComponent("Ds402Drive", config.log_level) + , client_(client) + , state_timeout_(config.state_timeout) + , poll_period_(config.poll_period) {} + + /// \brief Create a DS402 drive helper with the default configuration. + /// \param client The CANopen client for the drive's node. Must outlive this object. + explicit Ds402Drive(CanopenClient &client) + : Ds402Drive(client, Config{}) {} + + /// @name Standard object accessors + /// @{ + + /// \brief Read the device type (object 0x1000). \param ec Set on failure. \return The value. + uint32_t get_device_type(std::error_code &ec) { + return client_.read_u32(detail::ds402::OBJ_DEVICE_TYPE, 0, ec); + } + /// \brief Read the error register (object 0x1001). \param ec Set on failure. \return The value. + uint8_t get_error_register(std::error_code &ec) { + return client_.read_u8(detail::ds402::OBJ_ERROR_REGISTER, 0, ec); + } + /// \brief Read the manufacturer device name (object 0x1008, segmented upload). + /// \param ec Set on failure. \return The device name string. + std::string get_device_name(std::error_code &ec) { + return client_.read_string(detail::ds402::OBJ_DEVICE_NAME, 0, ec); + } + /// \brief Read the identity vendor id (object 0x1018:1). \param ec Set on failure. + /// \return The value. + uint32_t get_vendor_id(std::error_code &ec) { + return client_.read_u32(detail::ds402::OBJ_IDENTITY, 1, ec); + } + /// \brief Read the identity product code (object 0x1018:2). \param ec Set on failure. + /// \return The value. + uint32_t get_product_code(std::error_code &ec) { + return client_.read_u32(detail::ds402::OBJ_IDENTITY, 2, ec); + } + /// \brief Read the identity revision number (object 0x1018:3). \param ec Set on failure. + /// \return The value. + uint32_t get_revision_number(std::error_code &ec) { + return client_.read_u32(detail::ds402::OBJ_IDENTITY, 3, ec); + } + /// \brief Read the identity serial number (object 0x1018:4). \param ec Set on failure. + /// \return The value. + uint32_t get_serial_number(std::error_code &ec) { + return client_.read_u32(detail::ds402::OBJ_IDENTITY, 4, ec); + } + + /// \brief Write the controlword (object 0x6040). \param controlword Value to write. + /// \param ec Set on failure. \return True on success. + bool set_controlword(uint16_t controlword, std::error_code &ec) { + logger_.debug("controlword <- 0x{:04X}", controlword); + return client_.write_u16(detail::ds402::OBJ_CONTROLWORD, 0, controlword, ec); + } + /// \brief Read the statusword (object 0x6041). \param ec Set on failure. \return The value. + uint16_t get_statusword(std::error_code &ec) { + return client_.read_u16(detail::ds402::OBJ_STATUSWORD, 0, ec); + } + /// \brief Read the velocity actual value (object 0x606C). \param ec Set on failure. + /// \return The value. + int32_t get_velocity_actual(std::error_code &ec) { + return client_.read_i32(detail::ds402::OBJ_VELOCITY_ACTUAL, 0, ec); + } + /// \brief Read the position actual value (object 0x6064). \param ec Set on failure. + /// \return The value. + int32_t get_position_actual(std::error_code &ec) { + return client_.read_i32(detail::ds402::OBJ_POSITION_ACTUAL, 0, ec); + } + /// \brief Write the profile velocity (object 0x6081). \param velocity Value to write. + /// \param ec Set on failure. \return True on success. + bool set_profile_velocity(uint32_t velocity, std::error_code &ec) { + return client_.write_u32(detail::ds402::OBJ_PROFILE_VELOCITY, 0, velocity, ec); + } + /// \brief Write the profile acceleration (object 0x6083). \param acceleration Value to write. + /// \param ec Set on failure. \return True on success. + bool set_profile_acceleration(uint32_t acceleration, std::error_code &ec) { + return client_.write_u32(detail::ds402::OBJ_PROFILE_ACCELERATION, 0, acceleration, ec); + } + /// \brief Write the profile deceleration (object 0x6084). \param deceleration Value to write. + /// \param ec Set on failure. \return True on success. + bool set_profile_deceleration(uint32_t deceleration, std::error_code &ec) { + return client_.write_u32(detail::ds402::OBJ_PROFILE_DECELERATION, 0, deceleration, ec); + } + + /// @} + + /// @name State machine + /// @{ + + /// \brief Read the statusword and decode the CiA 402 drive state. + /// \param ec Set on failure (returns State::Unknown). + /// \return The decoded state. + State get_state(std::error_code &ec) { + const auto statusword = get_statusword(ec); + if (ec) { + return State::Unknown; + } + return detail::ds402::decode_state(statusword); + } + + /// \brief Walk the drive to Operation Enabled. + /// \details Issues Shutdown (0x0006) -> Switch On (0x0007) -> Enable + /// Operation (0x000F), polling the statusword after each command + /// until the corresponding state is reached or the configured + /// state_timeout expires. If the drive is in Fault, call + /// fault_reset() first. + /// \param ec Set on communication failure or transition timeout. + /// \return True once the drive reports Operation Enabled. + bool enable_operation(std::error_code &ec) { + auto state = get_state(ec); + if (ec) { + return false; + } + logger_.info("enable_operation: starting from state '{}'", + detail::ds402::state_to_string(state)); + if (state == State::Fault || state == State::FaultReactionActive) { + logger_.error("enable_operation: drive is in fault; call fault_reset() first"); + ec = std::make_error_code(std::errc::operation_not_permitted); + return false; + } + if (state != State::ReadyToSwitchOn && state != State::SwitchedOn && + state != State::OperationEnabled) { + if (!command_and_wait(detail::ds402::CW_SHUTDOWN, State::ReadyToSwitchOn, ec)) { + return false; + } + state = State::ReadyToSwitchOn; + } + if (state == State::ReadyToSwitchOn) { + if (!command_and_wait(detail::ds402::CW_SWITCH_ON, State::SwitchedOn, ec)) { + return false; + } + state = State::SwitchedOn; + } + if (state == State::SwitchedOn) { + if (!command_and_wait(detail::ds402::CW_ENABLE_OPERATION, State::OperationEnabled, ec)) { + return false; + } + } + logger_.info("enable_operation: drive is in Operation Enabled"); + return true; + } + + /// \brief Disable the drive (Shutdown command -> Ready to switch on, power stage off). + /// \param ec Set on communication failure or transition timeout. + /// \return True once the drive reports Ready to switch on. + bool disable(std::error_code &ec) { + return command_and_wait(detail::ds402::CW_SHUTDOWN, State::ReadyToSwitchOn, ec); + } + + /// \brief Issue a quick stop (controlword 0x0002). + /// \details Depending on the drive's quick-stop option code it transitions to + /// Quick Stop Active or directly to Switch On Disabled, so this does + /// not poll for a specific target state. + /// \param ec Set on communication failure. + /// \return True on success. + bool quick_stop(std::error_code &ec) { return set_controlword(detail::ds402::CW_QUICK_STOP, ec); } + + /// \brief Reset a drive fault (rising edge on controlword bit 7). + /// \details Writes controlword 0x0000 then 0x0080, then polls until the + /// drive leaves the Fault state. + /// \param ec Set on communication failure or if the fault persists. + /// \return True once the drive is no longer in Fault. + bool fault_reset(std::error_code &ec) { + if (!set_controlword(0x0000, ec)) { + return false; + } + if (!set_controlword(detail::ds402::CW_FAULT_RESET, ec)) { + return false; + } + return wait_for_state( + [](State s) { return s != State::Fault && s != State::FaultReactionActive; }, + "fault cleared", ec); + } + + /// \brief Set the mode of operation (object 0x6060) and verify via 0x6061. + /// \param mode The mode to select (e.g. OperatingMode::ProfileVelocity). + /// \param ec Set on communication failure or if the drive does not report the + /// mode within the state timeout. + /// \return True once modes-of-operation-display matches. + bool set_mode(OperatingMode mode, std::error_code &ec) { + if (!client_.write_i8(detail::ds402::OBJ_MODES_OF_OPERATION, 0, static_cast(mode), + ec)) { + return false; + } + const auto deadline = std::chrono::steady_clock::now() + state_timeout_; + do { + const auto display = client_.read_i8(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY, 0, ec); + if (ec) { + return false; + } + if (display == static_cast(mode)) { + return true; + } + std::this_thread::sleep_for(poll_period_); + } while (std::chrono::steady_clock::now() < deadline); + logger_.error("set_mode: drive did not report mode {} within {} ms", static_cast(mode), + state_timeout_.count()); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + + /// \brief Read the mode of operation display (object 0x6061). + /// \param ec Set on failure. \return The reported mode. + int8_t get_mode_display(std::error_code &ec) { + return client_.read_i8(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY, 0, ec); + } + + /// @} + + /// @name Motion + /// @{ + + /// \brief Write the target velocity (object 0x60FF; profile velocity mode). + /// \param velocity Target velocity in device units. + /// \param ec Set on failure. \return True on success. + bool set_target_velocity(int32_t velocity, std::error_code &ec) { + return client_.write_i32(detail::ds402::OBJ_TARGET_VELOCITY, 0, velocity, ec); + } + + /// \brief Command a profile-position move (object 0x607A + new-set-point handshake). + /// \details Writes the target position, then raises controlword bit 4 (new + /// set-point) with bit 5 (change set immediately) and bit 6 + /// (relative) as requested, waits for the drive to acknowledge via + /// statusword bit 12, and clears bit 4 again. The drive must already + /// be in Operation Enabled in profile position mode. + /// \param position Target position in device units. + /// \param ec Set on communication failure or acknowledge timeout. + /// \param immediate If true, the drive starts the new move immediately (bit 5). + /// \param relative If true, the target is relative to the current position (bit 6). + /// \return True once the set-point was acknowledged and bit 4 released. + bool set_target_position(int32_t position, std::error_code &ec, bool immediate = true, + bool relative = false) { + if (!client_.write_i32(detail::ds402::OBJ_TARGET_POSITION, 0, position, ec)) { + return false; + } + uint16_t controlword = detail::ds402::CW_ENABLE_OPERATION; + if (immediate) { + controlword |= detail::ds402::CW_BIT_CHANGE_SET_IMMEDIATELY; + } + if (relative) { + controlword |= detail::ds402::CW_BIT_RELATIVE; + } + // raise the new-set-point bit (4)... + if (!set_controlword(controlword | detail::ds402::CW_BIT_NEW_SETPOINT, ec)) { + return false; + } + // ...wait for set-point-acknowledge (statusword bit 12)... + const auto deadline = std::chrono::steady_clock::now() + state_timeout_; + while (true) { + const auto statusword = get_statusword(ec); + if (ec) { + return false; + } + if ((statusword & detail::ds402::SW_BIT_SETPOINT_ACKNOWLEDGE) != 0) { + break; + } + if (std::chrono::steady_clock::now() >= deadline) { + logger_.error("set_target_position: no set-point acknowledge within {} ms", + state_timeout_.count()); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + std::this_thread::sleep_for(poll_period_); + } + // ...and release the new-set-point bit again. + return set_controlword(controlword, ec); + } + + /// \brief Check whether the drive reports target reached (statusword bit 10). + /// \param ec Set on failure. \return True if the target is reached. + bool is_target_reached(std::error_code &ec) { + return (get_statusword(ec) & detail::ds402::SW_BIT_TARGET_REACHED) != 0; + } + + /// @} + +protected: + /// Write \p controlword, then poll until the drive reports \p target_state. + bool command_and_wait(uint16_t controlword, State target_state, std::error_code &ec) { + if (!set_controlword(controlword, ec)) { + return false; + } + return wait_for_state([target_state](State s) { return s == target_state; }, + detail::ds402::state_to_string(target_state), ec); + } + + /// Poll get_state() until \p predicate is satisfied or state_timeout_ expires. + bool wait_for_state(const std::function &predicate, const char *description, + std::error_code &ec) { + const auto deadline = std::chrono::steady_clock::now() + state_timeout_; + while (true) { + const auto state = get_state(ec); + if (ec) { + return false; + } + if (predicate(state)) { + return true; + } + if (std::chrono::steady_clock::now() >= deadline) { + logger_.error("timed out after {} ms waiting for '{}' (state is '{}')", + state_timeout_.count(), description, detail::ds402::state_to_string(state)); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + std::this_thread::sleep_for(poll_period_); + } + } + + CanopenClient &client_; + std::chrono::milliseconds state_timeout_; + std::chrono::milliseconds poll_period_; +}; + +} // namespace espp diff --git a/components/canopen/test/canopen_host_test.cpp b/components/canopen/test/canopen_host_test.cpp new file mode 100644 index 0000000000..8d8ab0e75c --- /dev/null +++ b/components/canopen/test/canopen_host_test.cpp @@ -0,0 +1,303 @@ +// Host-buildable unit tests for the CANopen (CiA 301) wire core and the +// CiA 402 statusword decoding. Build & run with: +// c++ -std=c++20 -I../include canopen_host_test.cpp -o test && ./test +// +// These tests exercise espp::detail::canopen / espp::detail::ds402 directly so +// they need no ESP-IDF headers. + +#include +#include +#include +#include +#include +#include + +#include "detail/canopen_core.hpp" + +namespace co = espp::detail::canopen; +namespace ds = espp::detail::ds402; +using espp::detail::CanFrame; + +static int g_failures = 0; +#define CHECK(cond) \ + do { \ + if (!(cond)) { \ + std::printf(" FAIL: %s (line %d)\n", #cond, __LINE__); \ + ++g_failures; \ + } \ + } while (0) + +static bool bytes_equal(const CanFrame &f, std::span expected) { + return f.dlc == expected.size() && std::equal(expected.begin(), expected.end(), f.data.begin()); +} + +static void test_nmt() { + std::printf("test_nmt\n"); + // golden: NMT start node 5 -> id 0x000, data [0x01, 0x05] + auto f = co::make_nmt(co::NmtCommand::Start, 5); + CHECK(f.id == 0x000); + CHECK(!f.extended && !f.rtr); + const uint8_t start5[] = {0x01, 0x05}; + CHECK(bytes_equal(f, start5)); + + // reset-node broadcast (all nodes) + f = co::make_nmt(co::NmtCommand::ResetNode, 0); + const uint8_t reset_all[] = {0x81, 0x00}; + CHECK(f.id == 0x000); + CHECK(bytes_equal(f, reset_all)); + + // stop / pre-operational / reset-communication command specifiers + CHECK(co::make_nmt(co::NmtCommand::Stop, 7).data[0] == 0x02); + CHECK(co::make_nmt(co::NmtCommand::PreOperational, 7).data[0] == 0x80); + CHECK(co::make_nmt(co::NmtCommand::ResetCommunication, 7).data[0] == 0x82); +} + +static void test_sync_and_pdo() { + std::printf("test_sync_and_pdo\n"); + auto sync = co::make_sync(); + CHECK(sync.id == 0x080); + CHECK(sync.dlc == 0); + + // RPDO1 to node 5, 4 bytes of packed data + const uint8_t payload[] = {0x11, 0x22, 0x33, 0x44}; + auto pdo = co::make_pdo(co::COB_RPDO1_BASE + 5, payload); + CHECK(pdo.id == 0x205); + CHECK(bytes_equal(pdo, payload)); +} + +static void test_heartbeat() { + std::printf("test_heartbeat\n"); + CanFrame f; + f.id = 0x705; + f.dlc = 1; + uint8_t node = 0; + + f.data[0] = 0x05; + auto state = co::parse_heartbeat(f, node); + CHECK(state.has_value() && *state == co::NmtState::Operational && node == 5); + + f.data[0] = 0x00; // boot-up + state = co::parse_heartbeat(f, node); + CHECK(state.has_value() && *state == co::NmtState::BootUp); + + f.data[0] = 0x7F; + state = co::parse_heartbeat(f, node); + CHECK(state.has_value() && *state == co::NmtState::PreOperational); + + f.data[0] = 0x04; + state = co::parse_heartbeat(f, node); + CHECK(state.has_value() && *state == co::NmtState::Stopped); + + // not a heartbeat: SDO response id + f.id = 0x585; + CHECK(!co::parse_heartbeat(f, node).has_value()); + // not a heartbeat: 0x700 itself is not a valid node id (node 0) + f.id = 0x700; + CHECK(!co::parse_heartbeat(f, node).has_value()); +} + +static void test_sdo_expedited_download() { + std::printf("test_sdo_expedited_download\n"); + // golden: write u32 0x12345678 to 0x60FF:00 on node 3 + // -> id 0x603, data [0x23, 0xFF, 0x60, 0x00, 0x78, 0x56, 0x34, 0x12] + const uint8_t val32[] = {0x78, 0x56, 0x34, 0x12}; // LE for 0x12345678 + auto f = co::make_sdo_expedited_download(3, 0x60FF, 0x00, val32); + CHECK(f.id == 0x603); + const uint8_t golden32[] = {0x23, 0xFF, 0x60, 0x00, 0x78, 0x56, 0x34, 0x12}; + CHECK(bytes_equal(f, golden32)); + + // u16 write -> command 0x2B; u8 write -> command 0x2F + const uint8_t val16[] = {0x0F, 0x00}; + f = co::make_sdo_expedited_download(3, 0x6040, 0x00, val16); + const uint8_t golden16[] = {0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00}; + CHECK(bytes_equal(f, golden16)); + + const uint8_t val8[] = {0x03}; + f = co::make_sdo_expedited_download(3, 0x6060, 0x00, val8); + const uint8_t golden8[] = {0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00}; + CHECK(bytes_equal(f, golden8)); + + // download response (scs=3) + CanFrame resp; + resp.id = 0x583; + resp.dlc = 8; + resp.data = {0x60, 0xFF, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; + auto r = co::parse_sdo_response(resp); + CHECK(r.type == co::SdoResponse::Type::DownloadOk); + CHECK(r.index == 0x60FF && r.subindex == 0x00); +} + +static void test_sdo_expedited_upload() { + std::printf("test_sdo_expedited_upload\n"); + // request: read 0x6041:00 from node 3 -> id 0x603, [0x40, 0x41, 0x60, 0x00, ...] + auto req = co::make_sdo_upload_request(3, 0x6041, 0x00); + CHECK(req.id == 0x603); + const uint8_t golden_req[] = {0x40, 0x41, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; + CHECK(bytes_equal(req, golden_req)); + + // response: expedited u16 0x0637 -> [0x4B, 0x41, 0x60, 0x00, 0x37, 0x06, 0, 0] + CanFrame resp; + resp.id = 0x583; + resp.dlc = 8; + resp.data = {0x4B, 0x41, 0x60, 0x00, 0x37, 0x06, 0x00, 0x00}; + auto r = co::parse_sdo_response(resp); + CHECK(r.type == co::SdoResponse::Type::ExpeditedUpload); + CHECK(r.index == 0x6041 && r.subindex == 0x00); + CHECK(r.len == 2); + CHECK(co::get_le(r.data.data(), r.len) == 0x0637); + + // response: expedited u32 (0x43) -> 4 valid bytes + resp.data = {0x43, 0x00, 0x10, 0x00, 0x92, 0x01, 0x02, 0x00}; + r = co::parse_sdo_response(resp); + CHECK(r.type == co::SdoResponse::Type::ExpeditedUpload); + CHECK(r.len == 4); + CHECK(co::get_le(r.data.data(), r.len) == 0x00020192); + + // response: expedited u8 (0x4F) + resp.data = {0x4F, 0x01, 0x10, 0x00, 0xA5, 0x00, 0x00, 0x00}; + r = co::parse_sdo_response(resp); + CHECK(r.type == co::SdoResponse::Type::ExpeditedUpload); + CHECK(r.len == 1); + CHECK(r.data[0] == 0xA5); +} + +static void test_sdo_abort() { + std::printf("test_sdo_abort\n"); + // abort: object does not exist (0x06020000) for 0x60FF:00 + CanFrame resp; + resp.id = 0x583; + resp.dlc = 8; + resp.data = {0x80, 0xFF, 0x60, 0x00, 0x00, 0x00, 0x02, 0x06}; + auto r = co::parse_sdo_response(resp); + CHECK(r.type == co::SdoResponse::Type::Abort); + CHECK(r.index == 0x60FF && r.subindex == 0x00); + CHECK(r.abort_code == 0x06020000); + CHECK(std::strcmp(co::sdo_abort_to_string(r.abort_code), + "object does not exist in the object dictionary") == 0); + CHECK(std::strcmp(co::sdo_abort_to_string(0x06090011), "subindex does not exist") == 0); + CHECK(std::strcmp(co::sdo_abort_to_string(0x06010002), "attempt to write a read-only object") == + 0); + CHECK(std::strcmp(co::sdo_abort_to_string(0x05040001), "invalid or unknown command specifier") == + 0); + CHECK(std::strcmp(co::sdo_abort_to_string(0xDEADBEEF), "unknown abort code") == 0); + + // abort frame builder (client-side abort, e.g. toggle error) + auto f = co::make_sdo_abort(3, 0x1008, 0x00, 0x05030000); + CHECK(f.id == 0x603); + const uint8_t golden[] = {0x80, 0x08, 0x10, 0x00, 0x00, 0x00, 0x03, 0x05}; + CHECK(bytes_equal(f, golden)); +} + +static void test_sdo_segmented_upload() { + std::printf("test_sdo_segmented_upload\n"); + // segmented upload of the 10-char device name "MCP266 2x6" from 0x1008:00 + const std::string name = "MCP266 2x6"; + CHECK(name.size() == 10); + + // initiate response: scs=2, e=0, s=1 -> 0x41, size = 10 + CanFrame init; + init.id = 0x583; + init.dlc = 8; + init.data = {0x41, 0x08, 0x10, 0x00, 0x0A, 0x00, 0x00, 0x00}; + auto r = co::parse_sdo_response(init); + CHECK(r.type == co::SdoResponse::Type::SegmentedUploadInit); + CHECK(r.index == 0x1008 && r.subindex == 0x00); + CHECK(r.size_indicated && r.total_size == 10); + + co::SdoSegmentedUpload assembler; + assembler.start(r); + CHECK(!assembler.done()); + CHECK(assembler.next_toggle() == false); + + // first segment request: toggle 0 -> 0x60 + auto req = co::make_sdo_upload_segment_request(3, assembler.next_toggle()); + CHECK(req.id == 0x603); + CHECK(req.data[0] == 0x60); + + // first segment response: toggle 0, 7 data bytes ("MCP266 "), not last + // -> byte0 = 0x00 | toggle<<4 | (7-7)<<1 | 0 = 0x00 + CanFrame seg1; + seg1.id = 0x583; + seg1.dlc = 8; + seg1.data = {0x00, 'M', 'C', 'P', '2', '6', '6', ' '}; + r = co::parse_sdo_response(seg1); + CHECK(r.type == co::SdoResponse::Type::UploadSegment); + CHECK(r.toggle == false && !r.last && r.len == 7); + CHECK(assembler.consume(r)); + CHECK(!assembler.done()); + CHECK(assembler.next_toggle() == true); + + // second segment request: toggle 1 -> 0x70 + req = co::make_sdo_upload_segment_request(3, assembler.next_toggle()); + CHECK(req.data[0] == 0x70); + + // second segment response: toggle 1, 3 data bytes ("2x6"), last + // -> byte0 = 0x00 | 1<<4 | (7-3)<<1 | 1 = 0x19 + CanFrame seg2; + seg2.id = 0x583; + seg2.dlc = 8; + seg2.data = {0x19, '2', 'x', '6', 0x00, 0x00, 0x00, 0x00}; + r = co::parse_sdo_response(seg2); + CHECK(r.type == co::SdoResponse::Type::UploadSegment); + CHECK(r.toggle == true && r.last && r.len == 3); + CHECK(assembler.consume(r)); + CHECK(assembler.done()); + CHECK(assembler.data() == name); + + // toggle-bit violation: replaying segment 1 (toggle 0) when 0 is expected + // again must be rejected once the assembler expects toggle 0 but gets 1 + co::SdoSegmentedUpload bad; + bad.start(co::parse_sdo_response(init)); + auto seg_toggle1 = co::parse_sdo_response(seg2); // toggle 1 first -> mismatch + CHECK(!bad.consume(seg_toggle1)); + CHECK(!bad.done()); +} + +static void test_le_helpers() { + std::printf("test_le_helpers\n"); + uint8_t buf[4] = {0, 0, 0, 0}; + co::put_le(0xAABBCCDD, buf, 4); + CHECK(buf[0] == 0xDD && buf[1] == 0xCC && buf[2] == 0xBB && buf[3] == 0xAA); + CHECK(co::get_le(buf, 4) == 0xAABBCCDD); + co::put_le(0x1234, buf, 2); + CHECK(buf[0] == 0x34 && buf[1] == 0x12); + CHECK(co::get_le(buf, 2) == 0x1234); +} + +static void test_ds402_decode() { + std::printf("test_ds402_decode\n"); + // vectors derived from the CiA 402 state masks (0x4F / 0x6F) + CHECK(ds::decode_state(0x0637) == ds::State::OperationEnabled); // 0x37 & 0x6F == 0x27 + CHECK(ds::decode_state(0x0200) == ds::State::NotReadyToSwitchOn); // & 0x4F == 0x00 + CHECK(ds::decode_state(0x0250) == ds::State::SwitchOnDisabled); // & 0x4F == 0x40 + CHECK(ds::decode_state(0x0631) == ds::State::ReadyToSwitchOn); // & 0x6F == 0x21 + CHECK(ds::decode_state(0x0633) == ds::State::SwitchedOn); // & 0x6F == 0x23 + CHECK(ds::decode_state(0x0617) == ds::State::QuickStopActive); // & 0x6F == 0x07 (bit 5 low) + CHECK(ds::decode_state(0x061F) == ds::State::FaultReactionActive); // & 0x4F == 0x0F + CHECK(ds::decode_state(0x0618) == ds::State::Fault); // & 0x4F == 0x08 + // higher (mode-specific) bits must not affect the decode + CHECK(ds::decode_state(0x1637) == ds::State::OperationEnabled); + CHECK(ds::decode_state(0xF250) == ds::State::SwitchOnDisabled); + + CHECK(std::strcmp(ds::state_to_string(ds::State::OperationEnabled), "Operation enabled") == 0); + CHECK(std::strcmp(ds::state_to_string(ds::State::Fault), "Fault") == 0); +} + +int main() { + test_nmt(); + test_sync_and_pdo(); + test_heartbeat(); + test_sdo_expedited_download(); + test_sdo_expedited_upload(); + test_sdo_abort(); + test_sdo_segmented_upload(); + test_le_helpers(); + test_ds402_decode(); + + if (g_failures == 0) { + std::printf("ALL TESTS PASSED\n"); + return 0; + } + std::printf("%d FAILURE(S)\n", g_failures); + return 1; +} diff --git a/doc/Doxyfile b/doc/Doxyfile index 0046d51535..0862a87168 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -92,6 +92,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/bq27220/example/main/bq27220_example.cpp \ $(PROJECT_PATH)/components/button/example/main/button_example.cpp \ $(PROJECT_PATH)/components/byte90/example/main/byte90_example.cpp \ + $(PROJECT_PATH)/components/canopen/example/main/canopen_example.cpp \ $(PROJECT_PATH)/components/chsc6x/example/main/chsc6x_example.cpp \ $(PROJECT_PATH)/components/cdr/example/main/cdr_example.cpp \ $(PROJECT_PATH)/components/cli/example/main/cli_example.cpp \ @@ -242,6 +243,9 @@ INPUT = \ $(PROJECT_PATH)/components/bq27220/include/bq27220.hpp \ $(PROJECT_PATH)/components/button/include/button.hpp \ $(PROJECT_PATH)/components/byte90/include/byte90.hpp \ + $(PROJECT_PATH)/components/canopen/include/canopen_client.hpp \ + $(PROJECT_PATH)/components/canopen/include/detail/canopen_core.hpp \ + $(PROJECT_PATH)/components/canopen/include/ds402.hpp \ $(PROJECT_PATH)/components/chsc6x/include/chsc6x.hpp \ $(PROJECT_PATH)/components/cdr/include/cdr.hpp \ $(PROJECT_PATH)/components/cli/include/cli.hpp \ diff --git a/doc/en/buses/canopen.rst b/doc/en/buses/canopen.rst new file mode 100644 index 0000000000..f207ef278a --- /dev/null +++ b/doc/en/buses/canopen.rst @@ -0,0 +1,45 @@ +CANopen (CiA 301) Client APIs +***************************** + +The `CanopenClient` class provides a lightweight, standards-based CANopen +(CiA 301) client / master for talking to a CANopen server node - for example a +Basicmicro MCP236/MCP266 motor controller - over a classic CAN 2.0 bus. It +implements NMT master commands, heartbeat / boot-up consumption, an SDO client +(expedited upload/download of 1/2/4-byte objects with typed accessors, plus +segmented upload for strings such as the manufacturer device name ``0x1008``), +RPDO transmit / TPDO reception dispatch, and SYNC transmission. + +The `Ds402Drive` class layers the CiA 402 (DS402) drive profile on top of a +`CanopenClient`: it decodes the statusword into the standard power-drive-system +states, walks the enable sequence (Shutdown -> Switch On -> Enable Operation) +with statusword polling and timeout, supports quick stop and fault reset, mode +selection (profile velocity / profile position / homing) verified via the modes +display object, and provides motion helpers such as ``set_target_velocity()`` +and ``set_target_position()`` (including the profile-position new-set-point +controlword handshake). + +The client is transport-agnostic: it transmits by invoking a user-provided +``send`` function with a plain ``espp::detail::CanFrame``, and the application +feeds received frames to ``process_frame()``. The ``CanFrame`` struct mirrors +``espp::Twai::Message`` field-for-field, so wiring it to the `Twai` component +is a trivial conversion (as the example shows) - but any CAN transport works. + +SDO transactions are blocking with a configurable timeout, so +``process_frame()`` must be called from a different task than the one +performing SDO transfers; with `Twai` this is automatically the case since its +``on_receive`` callback runs in the Twai receive task. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + canopen_example + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/canopen_client.inc +.. include-build-file:: inc/ds402.inc +.. include-build-file:: inc/canopen_core.inc diff --git a/doc/en/buses/canopen_example.md b/doc/en/buses/canopen_example.md new file mode 100644 index 0000000000..91867a0dd3 --- /dev/null +++ b/doc/en/buses/canopen_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/canopen/example/README.md +``` diff --git a/doc/en/buses/index.rst b/doc/en/buses/index.rst index 91c6c3ddf6..65a8b92972 100644 --- a/doc/en/buses/index.rst +++ b/doc/en/buses/index.rst @@ -11,4 +11,5 @@ external chips. spi rmt twai + canopen usb_cdc From d3d7d77b0b02800f3a5f2487d19a2cfe5350eba1 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 18 Aug 2026 19:00:54 -0500 Subject: [PATCH 2/5] fix(canopen): address PR #730 review comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - SDO responses are now matched to the in-flight request: process_frame() rejects responses whose echoed index/subindex do not match (or, for upload segments which carry no address, whose phase does not match) — a late response from a timed-out transaction can no longer complete the wrong one. Stale/unexpected responses are logged and ignored. - parse_sdo_response() requires dlc == 8 (CiA 301 SDO frames are exactly 8 bytes); shorter frames parse as Unknown instead of decoding missing bytes. Regression test added. - sdo_download() rejects payload sizes other than 1/2/4 (the only valid expedited sizes) with invalid_argument; builder precondition documented. - Typed reads (read_u16/u32/...) now require the response length to equal the requested width — read_u32 on a u16 object is a protocol_error instead of a silently-wrong value. - Profile-position handshake: after releasing the new-set-point bit, poll until set-point-acknowledge clears (bounded by the same timeout) so the next setpoint's rising edge is unambiguous. - ds402.hpp includes explicitly; manifest uses the https:// repository URL (github disabled the git:// protocol). Host tests ALL PASSED (incl. new malformed-dlc vectors); esp32 example builds; cppcheck clean. Co-Authored-By: Claude Opus 4.8 --- components/canopen/idf_component.yml | 2 +- components/canopen/include/canopen_client.hpp | 62 +++++++++++++++++-- .../canopen/include/detail/canopen_core.hpp | 8 ++- components/canopen/include/ds402.hpp | 28 ++++++++- components/canopen/test/canopen_host_test.cpp | 16 +++++ 5 files changed, 107 insertions(+), 9 deletions(-) diff --git a/components/canopen/idf_component.yml b/components/canopen/idf_component.yml index 09d88f52e6..6d9bc17a94 100644 --- a/components/canopen/idf_component.yml +++ b/components/canopen/idf_component.yml @@ -2,7 +2,7 @@ license: "MIT" description: "Lightweight CANopen (CiA 301) client / master with a DS402 (CiA 402) drive-profile helper" url: "https://github.com/esp-cpp/espp/tree/main/components/canopen" -repository: "git://github.com/esp-cpp/espp.git" +repository: "https://github.com/esp-cpp/espp.git" maintainers: - William Emfinger documentation: "https://esp-cpp.github.io/espp/buses/canopen.html" diff --git a/components/canopen/include/canopen_client.hpp b/components/canopen/include/canopen_client.hpp index 46cdda553c..0990768159 100644 --- a/components/canopen/include/canopen_client.hpp +++ b/components/canopen/include/canopen_client.hpp @@ -94,14 +94,39 @@ class CanopenClient : public BaseComponent { void process_frame(const CanFrame &frame) { // SDO response from our server node? if (frame.id == detail::canopen::COB_SDO_TX_BASE + node_id_) { + using Type = detail::canopen::SdoResponse::Type; + const auto parsed = detail::canopen::parse_sdo_response(frame); + bool delivered = false; { std::lock_guard lock(response_mutex_); if (awaiting_response_) { - response_ = detail::canopen::parse_sdo_response(frame); - awaiting_response_ = false; + // Only deliver a response that belongs to the in-flight request: a + // late response from a previously timed-out transaction (or any + // unrelated server traffic) must not complete the wrong one. + // Upload-segment responses carry no index/subindex, so they match by + // expected phase; every other response type echoes the object + // address and must match it. Malformed frames (Unknown) never match. + const bool is_segment = parsed.type == Type::UploadSegment; + bool matches = false; + if (expected_segment_) { + matches = is_segment; + } else { + matches = !is_segment && parsed.type != Type::Unknown && + parsed.index == expected_index_ && parsed.subindex == expected_subindex_; + } + if (matches) { + response_ = parsed; + awaiting_response_ = false; + delivered = true; + } } } - response_cv_.notify_all(); + if (delivered) { + response_cv_.notify_all(); + } else { + logger_.warn("Ignoring stale/unexpected SDO response (type {}, 0x{:04X}:{:02X})", + static_cast(parsed.type), parsed.index, parsed.subindex); + } return; } // heartbeat / boot-up from any node? @@ -225,6 +250,14 @@ class CanopenClient : public BaseComponent { /// \return True on success. bool sdo_download(uint16_t index, uint8_t subindex, std::span data, std::error_code &ec) { + // Expedited transfers are only defined for 1, 2 or 4 bytes (CiA 301); + // anything else would silently encode a wrong size on the wire. + if (data.size() != 1 && data.size() != 2 && data.size() != 4) { + logger_.error("SDO download 0x{:04X}:{:02X}: invalid expedited size {} (must be 1, 2 or 4)", + index, subindex, data.size()); + ec = std::make_error_code(std::errc::invalid_argument); + return false; + } std::lock_guard lock(sdo_mutex_); detail::canopen::SdoResponse response; if (!sdo_transact(detail::canopen::make_sdo_expedited_download(node_id_, index, subindex, data), @@ -292,7 +325,7 @@ class CanopenClient : public BaseComponent { while (!assembler.done()) { if (!sdo_transact( detail::canopen::make_sdo_upload_segment_request(node_id_, assembler.next_toggle()), - response, index, subindex, ec)) { + response, index, subindex, ec, /*expect_segment=*/true)) { return {}; } if (response.type != Type::UploadSegment || !assembler.consume(response)) { @@ -401,10 +434,17 @@ class CanopenClient : public BaseComponent { /// sdo_mutex_ held. On an abort response, logs the decoded abort reason and /// fails with protocol_error. bool sdo_transact(const CanFrame &request, detail::canopen::SdoResponse &response, uint16_t index, - uint8_t subindex, std::error_code &ec) { + uint8_t subindex, std::error_code &ec, bool expect_segment = false) { { std::lock_guard lock(response_mutex_); awaiting_response_ = true; + // Record what the in-flight request is for, so process_frame() can + // reject stale/unrelated responses instead of completing the wrong + // transaction (segment responses carry no index/subindex and are + // matched by phase instead). + expected_index_ = index; + expected_subindex_ = subindex; + expected_segment_ = expect_segment; } if (!send_frame(request, ec)) { std::lock_guard lock(response_mutex_); @@ -444,6 +484,15 @@ class CanopenClient : public BaseComponent { if (len == 0) { return 0; } + // The typed accessors promise the exact width they were asked for; a + // shorter response (e.g. read_u32 on a u16 object) would otherwise decode + // to a silently-wrong value. Treat any size mismatch as a protocol error. + if (len != num_bytes) { + logger_.error("SDO upload 0x{:04X}:{:02X}: object is {} bytes, expected {}", index, subindex, + len, num_bytes); + ec = std::make_error_code(std::errc::protocol_error); + return 0; + } return detail::canopen::get_le(data.data(), len); } @@ -459,6 +508,9 @@ class CanopenClient : public BaseComponent { mutable std::mutex response_mutex_; std::condition_variable response_cv_; bool awaiting_response_{false}; + uint16_t expected_index_{0}; // object address of the in-flight SDO request + uint8_t expected_subindex_{0}; // (used to reject stale/unrelated responses) + bool expected_segment_{false}; // in-flight request awaits an upload segment detail::canopen::SdoResponse response_{}; uint32_t last_abort_code_{0}; diff --git a/components/canopen/include/detail/canopen_core.hpp b/components/canopen/include/detail/canopen_core.hpp index f815e7e207..2125cdff49 100644 --- a/components/canopen/include/detail/canopen_core.hpp +++ b/components/canopen/include/detail/canopen_core.hpp @@ -164,6 +164,9 @@ inline std::optional parse_heartbeat(const CanFrame &frame, uint8_t &n /// \return The frame to transmit. inline CanFrame make_sdo_expedited_download(uint8_t node_id, uint16_t index, uint8_t subindex, std::span data) { + // PRECONDITION: data.size() must be 1, 2 or 4 — the only valid CiA 301 + // expedited-transfer sizes. CanopenClient::sdo_download() enforces this and + // fails with invalid_argument before calling here. CanFrame f; f.id = COB_SDO_RX_BASE + node_id; f.dlc = 8; @@ -250,7 +253,10 @@ struct SdoResponse { /// is unrecognized or the frame is malformed. inline SdoResponse parse_sdo_response(const CanFrame &frame) { SdoResponse r; - if (frame.dlc < 1) { + // CiA 301 specifies SDO frames as exactly 8 bytes; the field decoding below + // (index / subindex / abort code / data) assumes the full layout, so treat + // anything shorter as malformed rather than reading missing bytes. + if (frame.dlc != 8) { return r; } const uint8_t cmd = frame.data[0]; diff --git a/components/canopen/include/ds402.hpp b/components/canopen/include/ds402.hpp index ada2268100..7b8d1da9c3 100644 --- a/components/canopen/include/ds402.hpp +++ b/components/canopen/include/ds402.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -308,8 +309,31 @@ class Ds402Drive : public BaseComponent { } std::this_thread::sleep_for(poll_period_); } - // ...and release the new-set-point bit again. - return set_controlword(controlword, ec); + // ...release the new-set-point bit again... + if (!set_controlword(controlword, ec)) { + return false; + } + // ...and confirm the drive clears set-point-acknowledge before returning, + // so the next setpoint's rising edge is unambiguous (some drives hold the + // acknowledge bit until bit 4 is released; starting a new handshake with + // it still high can be silently ignored). + const auto ack_deadline = std::chrono::steady_clock::now() + state_timeout_; + while (true) { + const auto statusword = get_statusword(ec); + if (ec) { + return false; + } + if ((statusword & detail::ds402::SW_BIT_SETPOINT_ACKNOWLEDGE) == 0) { + return true; + } + if (std::chrono::steady_clock::now() >= ack_deadline) { + logger_.error("set_target_position: set-point acknowledge did not clear within {} ms", + state_timeout_.count()); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + std::this_thread::sleep_for(poll_period_); + } } /// \brief Check whether the drive reports target reached (statusword bit 10). diff --git a/components/canopen/test/canopen_host_test.cpp b/components/canopen/test/canopen_host_test.cpp index 8d8ab0e75c..af8f969940 100644 --- a/components/canopen/test/canopen_host_test.cpp +++ b/components/canopen/test/canopen_host_test.cpp @@ -283,6 +283,21 @@ static void test_ds402_decode() { CHECK(std::strcmp(ds::state_to_string(ds::State::Fault), "Fault") == 0); } +// SDO frames are specified as exactly 8 bytes (CiA 301); anything shorter must +// parse as Unknown instead of decoding missing bytes as protocol fields. +static void test_sdo_malformed_dlc() { + std::printf("test_sdo_malformed_dlc\n"); + CanFrame f; + f.id = 0x583; + f.data = {0x4B, 0x41, 0x60, 0x00, 0x37, 0x06, 0x00, 0x00}; + f.dlc = 7; // one byte short + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::Unknown); + f.dlc = 0; + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::Unknown); + f.dlc = 8; // and the full-length frame still parses + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::ExpeditedUpload); +} + int main() { test_nmt(); test_sync_and_pdo(); @@ -290,6 +305,7 @@ int main() { test_sdo_expedited_download(); test_sdo_expedited_upload(); test_sdo_abort(); + test_sdo_malformed_dlc(); test_sdo_segmented_upload(); test_le_helpers(); test_ds402_decode(); From b7c930a673e15f7160db532ebd39c8b645737b74 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 18 Aug 2026 20:44:27 -0500 Subject: [PATCH 3/5] fix(canopen): reject non-SDO frame types + static example lifetimes (PR #730 review, cppcheck) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - parse_sdo_response(): reject RTR and extended-id frames (which merely collide with the COB-ID) as malformed — an in-flight transaction can no longer be satisfied by a frame that is not a CiA 301 SDO data frame. Regression vectors added. - example: twai/client are function-local statics — the Twai receive task and the send lambda reference them and app_main() has early-return error paths, so static storage guarantees they outlive every callback (fixes the cppcheck danglingLifetime error on client_ptr). Host tests ALL PASSED; esp32 example builds; cppcheck clean. Co-Authored-By: Claude Opus 4.8 --- components/canopen/example/main/canopen_example.cpp | 8 ++++++-- components/canopen/include/detail/canopen_core.hpp | 9 +++++---- components/canopen/test/canopen_host_test.cpp | 8 ++++++++ 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/components/canopen/example/main/canopen_example.cpp b/components/canopen/example/main/canopen_example.cpp index 87bf0a15af..ffc6690275 100644 --- a/components/canopen/example/main/canopen_example.cpp +++ b/components/canopen/example/main/canopen_example.cpp @@ -29,7 +29,11 @@ extern "C" void app_main(void) { // task performing the (blocking) SDO transactions below -- which is exactly // what CanopenClient::process_frame() requires. The CanFrame struct mirrors // espp::Twai::Message field-for-field, so conversion is trivial. - espp::Twai twai({ + // NOTE: twai and client are function-local STATICS: the Twai receive task + // and the client's send lambda (which captures &twai) reference them, and + // app_main() has early-return error paths -- static storage guarantees they + // outlive every callback regardless of how app_main() exits. + static espp::Twai twai({ .tx_gpio = 5, // GPIO5 (change to match your board / transceiver) .rx_gpio = 4, // GPIO4 (change to match your board / transceiver) .baudrate = 250000, @@ -53,7 +57,7 @@ extern "C" void app_main(void) { // The CANopen client is transport-agnostic: give it a send function which // transmits an espp::detail::CanFrame (here: over TWAI). The CanFrame struct // mirrors espp::Twai::Message field-for-field, so conversion is trivial. - espp::CanopenClient client({ + static espp::CanopenClient client({ .node_id = node_id, .send = [&twai](const espp::CanopenClient::CanFrame &frame) { diff --git a/components/canopen/include/detail/canopen_core.hpp b/components/canopen/include/detail/canopen_core.hpp index 2125cdff49..adbb30ade2 100644 --- a/components/canopen/include/detail/canopen_core.hpp +++ b/components/canopen/include/detail/canopen_core.hpp @@ -253,10 +253,11 @@ struct SdoResponse { /// is unrecognized or the frame is malformed. inline SdoResponse parse_sdo_response(const CanFrame &frame) { SdoResponse r; - // CiA 301 specifies SDO frames as exactly 8 bytes; the field decoding below - // (index / subindex / abort code / data) assumes the full layout, so treat - // anything shorter as malformed rather than reading missing bytes. - if (frame.dlc != 8) { + // CiA 301 SDO frames are standard-id (11-bit) data frames of exactly 8 + // bytes; the field decoding below assumes that full layout. Reject RTR / + // extended-id frames (which merely happen to collide with the COB-ID) and + // short frames as malformed instead of decoding them as protocol data. + if (frame.dlc != 8 || frame.extended || frame.rtr) { return r; } const uint8_t cmd = frame.data[0]; diff --git a/components/canopen/test/canopen_host_test.cpp b/components/canopen/test/canopen_host_test.cpp index af8f969940..ec9432e1f6 100644 --- a/components/canopen/test/canopen_host_test.cpp +++ b/components/canopen/test/canopen_host_test.cpp @@ -296,6 +296,14 @@ static void test_sdo_malformed_dlc() { CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::Unknown); f.dlc = 8; // and the full-length frame still parses CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::ExpeditedUpload); + // extended-id / RTR frames that merely collide with the COB-ID are not SDO + f.extended = true; + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::Unknown); + f.extended = false; + f.rtr = true; + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::Unknown); + f.rtr = false; + CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::ExpeditedUpload); } int main() { From 9dc022583a324ab1e2587d283d4f773a555a2700 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 18 Aug 2026 20:46:04 -0500 Subject: [PATCH 4/5] fix(canopen): example statics must not be lambda-captured Follow-up to the danglingLifetime fix: capturing a static-storage variable is ill-formed (-Werror), so the send / heartbeat lambdas are now captureless and reference the static twai/logger directly (logger made static for the same lifetime reason). esp32 example builds; cppcheck clean. Co-Authored-By: Claude Opus 4.8 --- .../canopen/example/main/canopen_example.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/components/canopen/example/main/canopen_example.cpp b/components/canopen/example/main/canopen_example.cpp index ffc6690275..175307d5fa 100644 --- a/components/canopen/example/main/canopen_example.cpp +++ b/components/canopen/example/main/canopen_example.cpp @@ -8,7 +8,9 @@ using namespace std::chrono_literals; extern "C" void app_main(void) { - espp::Logger logger({.tag = "CANopen Example", .level = espp::Logger::Verbosity::INFO}); + // static: captured by the (static) client's heartbeat callback, which must + // outlive any early return from app_main() + static espp::Logger logger({.tag = "CANopen Example", .level = espp::Logger::Verbosity::INFO}); logger.info("Starting CANopen (CiA 301) client example!"); //! [canopen example] @@ -59,8 +61,10 @@ extern "C" void app_main(void) { // mirrors espp::Twai::Message field-for-field, so conversion is trivial. static espp::CanopenClient client({ .node_id = node_id, + // captureless: twai has static storage duration and is referenced + // directly (capturing a static is ill-formed under -Werror) .send = - [&twai](const espp::CanopenClient::CanFrame &frame) { + [](const espp::CanopenClient::CanFrame &frame) { espp::Twai::Message msg{ .id = frame.id, .extended = frame.extended, @@ -73,9 +77,10 @@ extern "C" void app_main(void) { }, .sdo_timeout = 100ms, .on_heartbeat = - [&logger](uint8_t hb_node, espp::CanopenClient::NmtState state) { - logger.info("Heartbeat from node {}: NMT state {}", hb_node, static_cast(state)); - }, + // captureless: logger has static storage duration (see above) + [](uint8_t hb_node, espp::CanopenClient::NmtState state) { + logger.info("Heartbeat from node {}: NMT state {}", hb_node, static_cast(state)); + }, .log_level = espp::Logger::Verbosity::INFO, }); client_ptr = &client; From 4e82a6f084f0d6f817c0461e7bf6c7b47c571155 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 19 Aug 2026 08:28:42 -0500 Subject: [PATCH 5/5] fix(canopen): cap remote-supplied segmented-upload sizes (PR #730 review) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The initiate response's total_size is remote-supplied (up to 4 GiB) and an un-sized transfer could grow indefinitely — either could exhaust memory on an embedded target. SdoSegmentedUpload::start() now takes a size cap (default 1 KiB — segmented uploads here serve small string objects) and refuses an oversized reservation; consume() enforces the same cap while appending so a lying/un-sized transfer cannot grow past it. read_string() aborts the transfer with 0x05040005 (out of memory) and fails with message_size when the indicated size exceeds the cap. Regression vectors added (4 GiB claim refused, growth past cap refused, explicit larger cap honored); start() is [[nodiscard]]. Host tests ALL PASSED (-Werror clean); esp32 example builds; cppcheck clean. Co-Authored-By: Claude Opus 4.8 --- components/canopen/include/canopen_client.hpp | 13 ++++++- .../canopen/include/detail/canopen_core.hpp | 28 +++++++++++++- components/canopen/test/canopen_host_test.cpp | 37 ++++++++++++++++++- 3 files changed, 73 insertions(+), 5 deletions(-) diff --git a/components/canopen/include/canopen_client.hpp b/components/canopen/include/canopen_client.hpp index 0990768159..5c6b4d711d 100644 --- a/components/canopen/include/canopen_client.hpp +++ b/components/canopen/include/canopen_client.hpp @@ -321,7 +321,18 @@ class CanopenClient : public BaseComponent { return {}; } detail::canopen::SdoSegmentedUpload assembler; - assembler.start(response); + if (!assembler.start(response)) { + // The remote-indicated total size exceeds the assembler's cap + // (remote-supplied, so never trusted for an unbounded reservation). + logger_.error("SDO segmented upload 0x{:04X}:{:02X}: indicated size {} exceeds the {} byte " + "cap; aborting transfer", + index, subindex, response.total_size, + detail::canopen::SdoSegmentedUpload::kDefaultMaxSize); + send_frame_quietly( + detail::canopen::make_sdo_abort(node_id_, index, subindex, 0x05040005)); // out of memory + ec = std::make_error_code(std::errc::message_size); + return {}; + } while (!assembler.done()) { if (!sdo_transact( detail::canopen::make_sdo_upload_segment_request(node_id_, assembler.next_toggle()), diff --git a/components/canopen/include/detail/canopen_core.hpp b/components/canopen/include/detail/canopen_core.hpp index adbb30ade2..4c2114cea1 100644 --- a/components/canopen/include/detail/canopen_core.hpp +++ b/components/canopen/include/detail/canopen_core.hpp @@ -315,15 +315,31 @@ inline SdoResponse parse_sdo_response(const CanFrame &frame) { /// (make_sdo_upload_segment_request with next_toggle()). class SdoSegmentedUpload { public: + /// Default cap on the total transfer size. total_size in the initiate + /// response is remote-supplied (up to 4 GiB), and an un-sized transfer could + /// otherwise grow indefinitely -- either would exhaust memory on an embedded + /// target. Segmented uploads here serve small objects (device name / version + /// strings), so the default is deliberately tight; pass a larger cap to + /// start() when a bigger object is genuinely expected. + static constexpr size_t kDefaultMaxSize = 1024; + /// \brief Start a transfer from a parsed SegmentedUploadInit response. /// \param init The parsed initiate response. - void start(const SdoResponse &init) { + /// \param max_size Maximum accepted total size (reservation AND growth cap). + /// \return False if the remote-indicated size exceeds \p max_size (the + /// transfer must then be aborted by the caller), true otherwise. + [[nodiscard]] bool start(const SdoResponse &init, size_t max_size = kDefaultMaxSize) { data_.clear(); toggle_ = false; done_ = false; + max_size_ = max_size; if (init.size_indicated) { + if (init.total_size > max_size_) { + return false; + } data_.reserve(init.total_size); } + return true; } /// \brief The toggle bit to use for the next segment request. @@ -331,11 +347,18 @@ class SdoSegmentedUpload { /// \brief Consume a parsed UploadSegment response. /// \param segment The parsed segment response. - /// \return False on toggle-bit mismatch (protocol error), true otherwise. + /// \return False on toggle-bit mismatch or if the accumulated data would + /// exceed the start() size cap (protocol error either way), true + /// otherwise. bool consume(const SdoResponse &segment) { if (segment.toggle != toggle_) { return false; } + // Enforce the cap while appending too: an un-sized (or lying) transfer + // must not grow past it. + if (data_.size() + segment.len > max_size_) { + return false; + } data_.append(reinterpret_cast(segment.data.data()), segment.len); toggle_ = !toggle_; done_ = segment.last; @@ -350,6 +373,7 @@ class SdoSegmentedUpload { private: std::string data_{}; + size_t max_size_{kDefaultMaxSize}; bool toggle_{false}; bool done_{false}; }; diff --git a/components/canopen/test/canopen_host_test.cpp b/components/canopen/test/canopen_host_test.cpp index ec9432e1f6..8f29029193 100644 --- a/components/canopen/test/canopen_host_test.cpp +++ b/components/canopen/test/canopen_host_test.cpp @@ -205,7 +205,7 @@ static void test_sdo_segmented_upload() { CHECK(r.size_indicated && r.total_size == 10); co::SdoSegmentedUpload assembler; - assembler.start(r); + CHECK(assembler.start(r)); CHECK(!assembler.done()); CHECK(assembler.next_toggle() == false); @@ -247,7 +247,7 @@ static void test_sdo_segmented_upload() { // toggle-bit violation: replaying segment 1 (toggle 0) when 0 is expected // again must be rejected once the assembler expects toggle 0 but gets 1 co::SdoSegmentedUpload bad; - bad.start(co::parse_sdo_response(init)); + CHECK(bad.start(co::parse_sdo_response(init))); auto seg_toggle1 = co::parse_sdo_response(seg2); // toggle 1 first -> mismatch CHECK(!bad.consume(seg_toggle1)); CHECK(!bad.done()); @@ -306,6 +306,38 @@ static void test_sdo_malformed_dlc() { CHECK(co::parse_sdo_response(f).type == co::SdoResponse::Type::ExpeditedUpload); } +// The initiate response's total_size is remote-supplied: the assembler must +// refuse an oversized reservation up front and stop an un-sized (or lying) +// transfer from growing past the cap while appending. +static void test_sdo_segmented_size_cap() { + std::printf("test_sdo_segmented_size_cap\n"); + co::SdoResponse init; + init.type = co::SdoResponse::Type::SegmentedUploadInit; + init.size_indicated = true; + init.total_size = 0xFFFFFFFFu; // 4 GiB claim + co::SdoSegmentedUpload a; + CHECK(!a.start(init)); // refused up front (default 1 KiB cap) + init.total_size = 2048; + CHECK(!a.start(init)); // still over the default cap + CHECK(a.start(init, 4096)); // fine under an explicit larger cap + + // un-sized transfer: growth is capped while appending + co::SdoResponse init2; + init2.type = co::SdoResponse::Type::SegmentedUploadInit; + init2.size_indicated = false; + co::SdoSegmentedUpload b; + CHECK(b.start(init2, 10)); // 10-byte cap + co::SdoResponse seg; + seg.type = co::SdoResponse::Type::UploadSegment; + seg.len = 7; + seg.last = false; + seg.toggle = false; + std::fill(seg.data.begin(), seg.data.end(), 0x41); + CHECK(b.consume(seg)); // 7 <= 10 + seg.toggle = true; + CHECK(!b.consume(seg)); // 14 > 10 -> refused +} + int main() { test_nmt(); test_sync_and_pdo(); @@ -314,6 +346,7 @@ int main() { test_sdo_expedited_upload(); test_sdo_abort(); test_sdo_malformed_dlc(); + test_sdo_segmented_size_cap(); test_sdo_segmented_upload(); test_le_helpers(); test_ds402_decode();