diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index aa7d7cb88..acfe24ff6 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -210,6 +210,8 @@ jobs: target: esp32s3 - path: 'components/odrive_ascii/example' target: esp32 + - path: 'components/odrive_native/example' + target: esp32 - path: 'components/pca9535/example' target: esp32s3 - path: 'components/pcf85063/example' diff --git a/.github/workflows/odrive_native_interop.yml b/.github/workflows/odrive_native_interop.yml new file mode 100644 index 000000000..2751b0ce1 --- /dev/null +++ b/.github/workflows/odrive_native_interop.yml @@ -0,0 +1,34 @@ +name: ODrive native interop (fibre serial loopback) + +# Minimal token scope: the harness only checks out, builds, and fetches the +# reference fibre client; it never writes. +permissions: + contents: read + +on: + pull_request: + paths: + - "components/odrive_native/**" + - "pc/tests/odrive_native_*" + - ".github/workflows/odrive_native_interop.yml" + workflow_dispatch: + +# Supersede in-progress runs on the same PR (or ref for manual dispatch); keyed by +# workflow + PR number (globally unique, unlike a head branch two forks can share). +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + interop: + runs-on: ubuntu-latest + timeout-minutes: 20 + steps: + - uses: actions/checkout@v7 + - uses: actions/setup-python@v5 + with: + python-version: "3.11" + - name: Run fibre serial-loopback interop + run: | + cd components/odrive_native/interop + ./run.sh diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 05453dc8e..53a87f6fe 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -123,6 +123,7 @@ jobs: components/neopixel components/nvs components/odrive_ascii + components/odrive_native components/pcf85063 components/pi4ioe5v components/pid diff --git a/components/odrive_native/CMakeLists.txt b/components/odrive_native/CMakeLists.txt new file mode 100644 index 000000000..43e9d353f --- /dev/null +++ b/components/odrive_native/CMakeLists.txt @@ -0,0 +1,8 @@ +# NOTE: unlike the vendored root-level detail/ folders in other espp components +# (format, cdr, hid-rp, ...), this component's detail/ lives INSIDE include/ +# (include/detail/*.hpp), so registering "include" alone makes +# `#include "detail/odrive_native_core.hpp"` resolve for consumers. +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES base_component +) diff --git a/components/odrive_native/PROTOCOL.md b/components/odrive_native/PROTOCOL.md new file mode 100644 index 000000000..2ddd451c8 --- /dev/null +++ b/components/odrive_native/PROTOCOL.md @@ -0,0 +1,102 @@ +# ODrive Native (legacy Fibre endpoint) protocol — implementation spec + +Authoritative wire spec for `espp::OdriveNative`, extracted from the ODrive +firmware reference (`fw-v0.5.1`): `Firmware/fibre/python/fibre/protocol.py` and +`Firmware/fibre/cpp/include/fibre/protocol.hpp`. + +**Target:** the legacy endpoint protocol (fw ≤ 0.5.x), **packet-based**, as used +over the USB **vendor** interface (one bulk IN + one bulk OUT). Each USB bulk +transfer carries exactly one packet (USB provides the reliability the UART +stream framing otherwise adds). Goal: `odrivetool` (legacy backend) +auto-discovers the object tree and does typed get/set. The newer 0.6+/Pro Fibre +is a different, larger stack and is out of scope for now. + +## Constants +- `PROTOCOL_VERSION = 1` +- CRC8: init `0x42`, poly `0x37` (only used by the UART *stream* framing) +- CRC16: init `0x1337`, poly `0x3d65` + +## CRC algorithm (both widths, **non-reflected, MSB-first, bit-by-bit**) +``` +calc_crc(remainder, byte, poly, bitwidth): # byte in [0,255] + topbit = 1 << (bitwidth - 1) + remainder ^= byte << (bitwidth - 8) + repeat 8 times: + remainder = (remainder & topbit) ? ((remainder << 1) ^ poly) + : (remainder << 1) + return remainder & ((1 << bitwidth) - 1) +# CRC over a buffer: start from init, fold each byte through calc_crc. +``` + +## Packet format (host ⇄ device, little-endian throughout) +**Request** (host → device): +``` +[seq_no u16 LE] # MSB (0x8000) clear in requests; client sets bit 0x80, masks 0x7fff +[endpoint_id u16 LE] # bit15 (0x8000) set => client expects a response; low 15 bits = endpoint # +[output_len u16 LE] # number of response bytes the client wants back +[payload ... ] # bytes to WRITE, or the read OFFSET (u32 LE) for endpoint 0; empty for a plain read +[trailer u16 LE] # canary: PROTOCOL_VERSION(1) if endpoint#==0, else json_crc +``` +**Response** (device → host, emitted only if endpoint_id bit15 was set): +``` +[seq_no u16 LE] # = request seq_no with MSB (0x8000) set +[data ... ] # up to output_len bytes (the value / json chunk); empty for a pure write +``` +The server **must ignore** a request whose `trailer` != the expected canary +(PROTOCOL_VERSION for endpoint 0, else `json_crc`) — this is how the client and +server confirm they share the same object model. + +## Server dispatch +- **endpoint 0** (the JSON blob): `payload` = offset (u32 LE). Respond with + `json[offset : offset + min(output_len, 512)]`. `offset >= len(json)` → empty + response (that is how the client's read loop terminates). trailer == PROTOCOL_VERSION. +- **endpoint N in registry**: if `payload` non-empty → deserialize per type and + **write** (when writable); if `output_len > 0` → serialize the current value + (per type, `output_len` bytes) into the response. trailer == `json_crc`. +- **unknown endpoint**: ignore (empty response). + +## Type codecs (little-endian) +| type | size | notes | +|---------------|------|------------------| +| `bool` | 1 | 0/1 | +| `int8`/`uint8`| 1 | | +| `int16`/`uint16` | 2 | | +| `int32`/`uint32` | 4 | | +| `int64`/`uint64` | 8 | | +| `float` | 4 | IEEE-754 | +| `endpoint_ref`| 4 | `[endpoint u16][json_crc u16]` | + +## JSON descriptor (the endpoint-0 blob) +Compact UTF-8 JSON (**no insignificant whitespace** — `json_crc` is over the +exact bytes). Top level is an **array** of the root object's members. Entries: +- property: `{"name":,"id":,"type":,"access":"r"|"rw"|"w"}` +- object: `{"name":,"type":"object","members":[ ... ]}` +- function: `{"name":,"id":,"type":"function","inputs":[...],"outputs":[...]}` + +`json_crc` = `calc_crc16(json_bytes, init=PROTOCOL_VERSION=1)` — the endpoint +canary is seeded with `PROTOCOL_VERSION`, **not** the 0x1337 packet-CRC init. This +matches the fw-v0.5.1 firmware (`endpoints_template.j2`: +`json_crc_ = calc_crc16(PROTOCOL_VERSION, embedded_json, len)`) and the reference +fibre client (`discovery.py`: `calc_crc16(PROTOCOL_VERSION, json_bytes)`). The +server computes it over the bytes it emits; `odrivetool` computes it over the +bytes it reads; they must match byte-for-byte. (Verified by the serial-loopback +interop harness in `interop/`; only the 0x1337 init applies to the UART *stream* +framing CRC16 and the packet trailer of endpoint 0, which is `PROTOCOL_VERSION`.) + +## `espp::OdriveNative` (transport-agnostic, mirrors `espp::OdriveAscii`) +- `std::vector process_bytes(std::span)` — one packet in, + one response packet out (empty if none). The caller performs USB I/O. +- Registration builds a typed endpoint tree (getters/setters via `std::function`, + `std::error_code`, no exceptions); ids are assigned and the JSON + `json_crc` + are finalized at build time. Names use dotted paths like + `axis0.controller.input_pos`, mirroring the ASCII component. + +## Verification plan +1. **CRC golden vectors** generated from the exact Python reference (above) — the + C++ `calc_crc16` must match bit-for-bit. +2. **Packet round-trip** host unit tests: crafted read / write / endpoint-0-read + requests → exact expected response bytes. +3. **Real interop** (later phase, the true gate): run genuine `fibre-python` / + `odrivetool` against the host build over a loopback and confirm it enumerates + the tree and reads/writes endpoints — mirrors how `rtps` is gated against + FastDDS / ROS 2. diff --git a/components/odrive_native/README.md b/components/odrive_native/README.md new file mode 100644 index 000000000..f4bd59c27 --- /dev/null +++ b/components/odrive_native/README.md @@ -0,0 +1,72 @@ +# ODrive Native (Fibre endpoint) Protocol Component + +[![Badge](https://components.espressif.com/components/espp/odrive_native/badge.svg)](https://components.espressif.com/components/espp/odrive_native) + +`espp::OdriveNative` implements a transport-agnostic server for the **ODrive +legacy native (Fibre endpoint) binary protocol** (firmware <= 0.5.x), as used +over the USB vendor interface where each bulk transfer carries exactly one +packet. It parses one inbound request packet and produces one response packet; +it performs no I/O itself (the caller does USB/UART transport). + +Applications register typed properties from dotted paths (mirroring +`espp::OdriveAscii`). Endpoint ids are assigned sequentially starting at 1 +(endpoint 0 is the JSON descriptor blob), and the compact JSON descriptor and +its CRC are finalized lazily. This lets a legacy `odrivetool` / `fibre-python` +client auto-discover the object tree and perform typed get/set. + + +**Table of Contents** + +- [ODrive Native (Fibre endpoint) Protocol Component](#odrive-native-fibre-endpoint-protocol-component) + - [Features](#features) + - [API](#api) + - [Protocol](#protocol) + - [Example](#example) + - [Notes](#notes) + + + +## Features + +- **Transport-agnostic**: one packet in via `process_bytes`, one response packet out +- **Typed property registry**: `register_float_property`, and + `_int8_/_uint8_/_int16_/_uint16_/_int32_/_uint32_/_int64_/_uint64_/_bool_` + variants, each taking a getter and optional setter (no exceptions; uses + `std::error_code`) +- **Auto-discovery**: builds the endpoint-0 JSON descriptor + `json_crc` so a + legacy Fibre client can enumerate the tree +- **No hardware dependencies**: integrates via `std::function` callbacks +- **Thread-safe**: internal locking for the registry; user getters/setters are + never invoked while a lock is held (snapshot then call) +- **Host-buildable wire core**: the CRC/pack/codec/JSON/dispatch logic lives in + `detail::OdriveNativeCore`, which builds with just the standard library + +## API + +Key class: `espp::OdriveNative` +- `process_bytes(std::span) -> std::vector` (one packet in, one out) +- Register properties: `register_float_property`, `register_int32_property`, + `register_uint32_property`, `register_bool_property`, and the other integer + width variants +- `finalize()`, `json()`, `json_crc()` inspect the generated descriptor + +See header [`include/odrive_native.hpp`](./include/odrive_native.hpp) and +[`PROTOCOL.md`](./PROTOCOL.md) for details. + +## Protocol + +The authoritative wire specification (packet format, CRC, endpoint dispatch, +type codecs, and JSON schema) is documented in [`PROTOCOL.md`](./PROTOCOL.md). + +## Example + +A scripted example is provided in [`example`](./example) and is built by CI. It +registers a few simulated-motor properties and feeds crafted packets through +`process_bytes`, logging the responses. + +## Notes + +- This component implements the **properties** (primitive get/set) surface of the + legacy protocol; functions / endpoint refs are not implemented yet. +- Wiring to a concrete USB device stack is a later phase; this component is + purely the protocol server. diff --git a/components/odrive_native/example/CMakeLists.txt b/components/odrive_native/example/CMakeLists.txt new file mode 100644 index 000000000..fd21dd36f --- /dev/null +++ b/components/odrive_native/example/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "${CMAKE_CURRENT_LIST_DIR}/../.." +) + +set( + COMPONENTS + "main esptool_py odrive_native" + CACHE STRING + "List of components to include" + ) + +project(odrive_native_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/odrive_native/example/README.md b/components/odrive_native/example/README.md new file mode 100644 index 000000000..fe983fedf --- /dev/null +++ b/components/odrive_native/example/README.md @@ -0,0 +1,50 @@ +# ODrive Native (Fibre endpoint) Example + +This example demonstrates how to use the `espp::OdriveNative` component to serve +the ODrive legacy native (Fibre endpoint) binary protocol. It registers a few +simulated-motor properties, then feeds crafted request packets through +`process_bytes` and logs the responses: + +1. an endpoint-0 read that returns the auto-generated JSON descriptor, +2. a binary write to `axis0.controller.input_pos`, and +3. a binary read of the same property. + + +**Table of Contents** + +- [ODrive Native (Fibre endpoint) Example](#odrive-native-fibre-endpoint-example) + - [Requirements](#requirements) + - [Build](#build) + - [Flash and Monitor](#flash-and-monitor) + - [Notes](#notes) + + + +## Requirements + +- ESP-IDF installed and `get_idf` available in your shell + +## Build + +```sh +# From repo root +cd components/odrive_native/example +get_idf +idf.py build +``` + +## Flash and Monitor + +```sh +idf.py flash monitor +``` + +The example runs a scripted packet sequence and logs the descriptor and the +per-packet responses. + +## Notes + +- The component is transport-agnostic: this example fabricates packets in code. + In a real deployment each USB bulk transfer would carry one packet, which you + pass to `process_bytes`, transmitting the returned response bytes back. +- Wiring to a concrete USB device stack is a later phase. diff --git a/components/odrive_native/example/main/CMakeLists.txt b/components/odrive_native/example/main/CMakeLists.txt new file mode 100644 index 000000000..4b68de3f0 --- /dev/null +++ b/components/odrive_native/example/main/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRC_DIRS "." + INCLUDE_DIRS "." +) diff --git a/components/odrive_native/example/main/odrive_native_example.cpp b/components/odrive_native/example/main/odrive_native_example.cpp new file mode 100644 index 000000000..1ff7c9d91 --- /dev/null +++ b/components/odrive_native/example/main/odrive_native_example.cpp @@ -0,0 +1,127 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "logger.hpp" +#include "odrive_native.hpp" + +using namespace espp; + +namespace { +// Little-endian packet builder helpers. +void put_u16(std::vector &v, uint16_t x) { + v.push_back(uint8_t(x & 0xff)); + v.push_back(uint8_t((x >> 8) & 0xff)); +} +void put_u32(std::vector &v, uint32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(uint8_t((x >> (8 * i)) & 0xff)); +} + +// Build a request packet: [seq][endpoint|resp_bit][output_len][payload][trailer] +std::vector make_packet(uint16_t seq, uint16_t endpoint_id, bool expect_response, + uint16_t output_len, std::span payload, + uint16_t trailer) { + std::vector p; + put_u16(p, seq); + put_u16(p, uint16_t(endpoint_id | (expect_response ? 0x8000 : 0))); + put_u16(p, output_len); + p.insert(p.end(), payload.begin(), payload.end()); + put_u16(p, trailer); + return p; +} + +std::string to_hex(const std::vector &v) { + std::string s; + char buf[4]; + for (uint8_t b : v) { + snprintf(buf, sizeof(buf), "%02x ", b); + s += buf; + } + return s; +} +} // namespace + +extern "C" void app_main(void) { + Logger logger({.tag = "ODriveNativeExample", .level = Logger::Verbosity::INFO}); + + //! [odrive_native_basic_example] + + // Simulated motor state. + struct { + float vbus_voltage = 24.0f; + float input_pos = 0.0f; + int32_t axis_state = 1; + } state; + + OdriveNative::Config cfg; + cfg.log_level = Logger::Verbosity::INFO; + OdriveNative proto(cfg); + + // Register a small object tree of simulated-motor properties. Endpoint ids + // are assigned in registration order starting at 1. + proto.register_float_property("vbus_voltage", [&]() { return state.vbus_voltage; }); // id 1 + proto.register_float_property( + "axis0.controller.input_pos", [&]() { return state.input_pos; }, // id 2 (rw) + [&](float v, std::error_code &ec) { + ec.clear(); + state.input_pos = v; + return true; + }); + proto.register_int32_property( + "axis0.current_state", [&]() { return state.axis_state; }, // id 3 (rw) + [&](int32_t v, std::error_code &ec) { + ec.clear(); + state.axis_state = v; + return true; + }); + + const uint16_t json_crc = proto.json_crc(); + logger.info("Endpoint JSON descriptor ({} bytes, crc=0x{:04x}):\n{}", proto.json().size(), + json_crc, proto.json()); + + // 1) endpoint-0 read: fetch the JSON descriptor (offset 0, up to 512 bytes). + { + std::vector offset; + put_u32(offset, 0); + auto req = make_packet(0x0001, /*endpoint*/ 0, /*expect*/ true, /*output_len*/ 512, offset, + /*trailer*/ 1 /*PROTOCOL_VERSION*/); + auto resp = proto.process_bytes(req); + std::string json(resp.begin() + 2, resp.end()); + logger.info("endpoint-0 read -> {} bytes: {}", resp.size(), json); + } + + // 2) write axis0.controller.input_pos (endpoint 2) = 3.14f. + { + const float value = 3.14f; + std::vector payload(4); + std::memcpy(payload.data(), &value, 4); + auto req = + make_packet(0x0002, /*endpoint*/ 2, /*expect*/ true, /*output_len*/ 0, payload, json_crc); + (void)proto.process_bytes(req); + logger.info("wrote input_pos=3.14 -> state.input_pos={}", state.input_pos); + } + + // 3) read axis0.controller.input_pos back (endpoint 2, output_len=4). + { + auto req = make_packet(0x0003, /*endpoint*/ 2, /*expect*/ true, /*output_len*/ 4, + std::span{}, json_crc); + auto resp = proto.process_bytes(req); + float readback = 0.0f; + if (resp.size() >= 6) + std::memcpy(&readback, resp.data() + 2, 4); + logger.info("read input_pos -> resp [{}] value={}", to_hex(resp), readback); + } + + //! [odrive_native_basic_example] + + logger.info("ODrive native example complete."); + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +} diff --git a/components/odrive_native/example/sdkconfig.defaults b/components/odrive_native/example/sdkconfig.defaults new file mode 100644 index 000000000..c3667f3e3 --- /dev/null +++ b/components/odrive_native/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/odrive_native/example/sdkconfig.defaults.esp32s3 b/components/odrive_native/example/sdkconfig.defaults.esp32s3 new file mode 100644 index 000000000..eaee743c2 --- /dev/null +++ b/components/odrive_native/example/sdkconfig.defaults.esp32s3 @@ -0,0 +1,3 @@ +# on the ESP32S3, which has native USB, we need to set the console so that the +# CLI can be configured correctly: +CONFIG_ESP_CONSOLE_USB_SERIAL_JTAG=y diff --git a/components/odrive_native/idf_component.yml b/components/odrive_native/idf_component.yml new file mode 100644 index 000000000..328023b5d --- /dev/null +++ b/components/odrive_native/idf_component.yml @@ -0,0 +1,24 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "ODrive legacy native (Fibre endpoint) binary protocol server component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/odrive_native" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/motor_control/odrive_native.html" +examples: + - path: example +tags: + - cpp + - Component + - ODrive + - Fibre + - Motor + - BLDC + - Binary + - Protocol + - USB +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' diff --git a/components/odrive_native/include/detail/odrive_native_core.hpp b/components/odrive_native/include/detail/odrive_native_core.hpp new file mode 100644 index 000000000..3eee5730d --- /dev/null +++ b/components/odrive_native/include/detail/odrive_native_core.hpp @@ -0,0 +1,520 @@ +#pragma once + +// ODrive legacy native (Fibre endpoint) protocol — wire core. +// +// This header is intentionally free of any ESP-IDF / FreeRTOS dependency so +// that the wire logic (CRC, packet packing, type codecs, JSON descriptor, +// endpoint dispatch) can be built and unit-tested on a host with nothing more +// than a C++20 standard library. The `espp::OdriveNative` component composes +// this core together with `espp::BaseComponent` for logging. +// +// See PROTOCOL.md for the authoritative wire specification. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace espp { +namespace detail { + +/// ODrive legacy CRC-16 (poly 0x3d65, init 0x1337, non-reflected, MSB-first). +/// Fold a single byte through the running remainder. +inline uint16_t odrive_crc16_byte(uint16_t rem, uint8_t val) { + rem ^= static_cast(static_cast(val) << 8); + for (int i = 0; i < 8; i++) + rem = (rem & 0x8000) ? static_cast((rem << 1) ^ 0x3d65) + : static_cast(rem << 1); + return rem; +} + +/// CRC-16 over a buffer using the ODrive init value (0x1337). +inline uint16_t odrive_crc16(std::span data, uint16_t init = 0x1337) { + return std::accumulate(data.begin(), data.end(), init, odrive_crc16_byte); +} + +/// CRC-16 convenience overload for a string_view. +inline uint16_t odrive_crc16(std::string_view s, uint16_t init = 0x1337) { + return odrive_crc16( + std::span(reinterpret_cast(s.data()), s.size()), init); +} + +/// The legacy protocol version (canary for endpoint 0). +static constexpr uint16_t kProtocolVersion = 1; + +/// Endianness helpers. Both ESP32 and the host dev machines are little-endian, +/// and the wire format is little-endian, so a raw byte copy is correct. The +/// static_assert guards against ever building on a big-endian target. C++20's +/// std::endian gives a definitive compile-time answer on every conforming +/// toolchain (no reliance on the compiler-specific __BYTE_ORDER__ macro). +static_assert(std::endian::native == std::endian::little, + "OdriveNative wire core assumes a little-endian target"); + +inline uint16_t read_u16_le(std::span s, size_t off) { + return static_cast(s[off]) | (static_cast(s[off + 1]) << 8); +} + +inline void append_u16_le(std::vector &v, uint16_t val) { + v.push_back(static_cast(val & 0xff)); + v.push_back(static_cast((val >> 8) & 0xff)); +} + +template inline void append_le(std::vector &v, T val) { + static_assert(std::is_trivially_copyable_v, "append_le requires trivially copyable type"); + uint8_t buf[sizeof(T)]; + std::memcpy(buf, &val, sizeof(T)); + for (size_t i = 0; i < sizeof(T); ++i) + v.push_back(buf[i]); +} + +template inline bool read_le(std::span s, T &out) { + static_assert(std::is_trivially_copyable_v, "read_le requires trivially copyable type"); + if (s.size() < sizeof(T)) + return false; + uint8_t buf[sizeof(T)]; + for (size_t i = 0; i < sizeof(T); ++i) + buf[i] = s[i]; + std::memcpy(&out, buf, sizeof(T)); + return true; +} + +/// Escape a string for inclusion in the compact JSON descriptor. Endpoint names +/// are normally plain identifiers, but escaping keeps json_crc correct if a +/// name ever contains a quote or backslash. +inline std::string json_escape(std::string_view s) { + std::string out; + out.reserve(s.size()); + for (char c : s) { + switch (c) { + case '"': + out += "\\\""; + break; + case '\\': + out += "\\\\"; + break; + case '\n': + out += "\\n"; + break; + case '\r': + out += "\\r"; + break; + case '\t': + out += "\\t"; + break; + default: + out += c; + break; + } + } + return out; +} + +/** + * @brief Transport-agnostic server for the ODrive legacy native (Fibre + * endpoint) binary protocol. + * + * This is the host-buildable core. Register typed properties from dotted paths; + * the core assigns sequential endpoint ids (starting at 1; endpoint 0 is the + * JSON descriptor), builds the compact JSON descriptor and its CRC, and + * dispatches inbound packets via process_bytes(). + */ +class OdriveNativeCore { +public: + /// Read accessor: return the current typed value. + template using getter_fn = std::function; + /// Write accessor: apply a typed value, set ec on error, return true on ok. + template using setter_fn = std::function; + /// Callback invoked with a human-readable message when a request is dropped + /// or a write fails. The wire protocol has no error channel, so without this + /// hook such failures are invisible to the device application. (Kept as a + /// plain std::function so this core stays ESP-free; espp::OdriveNative wires + /// it to its logger.) + using error_callback_fn = std::function; + + OdriveNativeCore() = default; + + /// Set the (optional) error callback. May be invoked from whatever context + /// calls process_bytes(); keep it short and non-blocking. + void set_error_callback(const error_callback_fn &cb) { + std::scoped_lock lk(mutex_); + on_error_ = cb; + } + + void register_float_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "float", getter, setter); + } + void register_int8_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "int8", getter, setter); + } + void register_uint8_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "uint8", getter, setter); + } + void register_int16_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "int16", getter, setter); + } + void register_uint16_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "uint16", getter, setter); + } + void register_int32_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "int32", getter, setter); + } + void register_uint32_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "uint32", getter, setter); + } + void register_int64_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "int64", getter, setter); + } + void register_uint64_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + register_typed(path, "uint64", getter, setter); + } + + /// Register a bool property. Wire size is 1 byte, serialized as 0/1. + void register_bool_property(const std::string &path, const getter_fn &getter, + const setter_fn &setter = nullptr) { + // An endpoint with neither accessor cannot be read or written; registering + // it would misrepresent its access as "r" in the schema. Reject it. + if (!getter && !setter) + return; + std::scoped_lock lk(mutex_); + // ids >= 0x8000 collide with the expect-response bit and are unreachable + // (the dispatcher masks the endpoint field with 0x7fff) + if (next_id_ >= 0x8000) { + if (on_error_) + on_error_("endpoint id space exhausted (max 32767); '" + path + "' not registered"); + return; + } + Endpoint ep; + ep.id = next_id_++; + ep.path = path; + ep.type = "bool"; + ep.size = 1; + ep.readable = static_cast(getter); + ep.writable = static_cast(setter); + if (getter) { + ep.serialize = [getter](std::vector &v) { v.push_back(getter() ? 1 : 0); }; + } + if (setter) { + ep.deserialize = [setter](std::span s) -> bool { + if (s.empty()) + return false; + std::error_code ec; + return setter(s[0] != 0, ec); + }; + } + endpoints_.push_back(std::move(ep)); + finalized_ = false; + } + + /// Build (or rebuild) the JSON descriptor and its CRC. Called lazily by + /// process_bytes(); safe to call explicitly. + void finalize() { + std::scoped_lock lk(mutex_); + finalize_locked(); + } + + /// The compact JSON descriptor bytes (endpoint 0 blob). + std::string json() { + std::scoped_lock lk(mutex_); + finalize_locked(); + return json_; + } + + /// CRC-16 over the JSON descriptor (the canary for endpoints > 0). + uint16_t json_crc() { + std::scoped_lock lk(mutex_); + finalize_locked(); + return json_crc_; + } + + /** + * @brief Process exactly one inbound packet and return the response packet. + * @param in One complete request packet (one USB bulk transfer). + * @return Response packet bytes, or empty if no response is expected / the + * packet is ignored. + */ + std::vector process_bytes(std::span in) { + // Minimum packet: seq(2) + endpoint(2) + output_len(2) + trailer(2). + if (in.size() < 8) + return {}; + + const uint16_t seq_no = read_u16_le(in, 0); + const uint16_t endpoint_field = read_u16_le(in, 2); + const uint16_t output_len = read_u16_le(in, 4); + const bool expect_response = (endpoint_field & 0x8000) != 0; + const uint16_t endpoint_id = endpoint_field & 0x7fff; + const uint16_t trailer = read_u16_le(in, in.size() - 2); + const std::span payload = in.subspan(6, in.size() - 8); + + // Snapshot everything we need under the lock, then invoke user callbacks + // (getter/setter) with the lock released. + std::string json_snapshot; + uint16_t json_crc_snapshot = 0; + bool have_endpoint = false; + bool writable = false; + size_t ep_size = 0; + std::string ep_path; + std::function &)> serialize; + std::function)> deserialize; + error_callback_fn on_error; + { + std::scoped_lock lk(mutex_); + finalize_locked(); + json_crc_snapshot = json_crc_; + on_error = on_error_; + if (endpoint_id == 0) { + json_snapshot = json_; + } else { + auto it = std::find_if(endpoints_.begin(), endpoints_.end(), + [endpoint_id](const Endpoint &ep) { return ep.id == endpoint_id; }); + if (it != endpoints_.end()) { + have_endpoint = true; + writable = it->writable; + ep_size = it->size; + ep_path = it->path; + serialize = it->serialize; + deserialize = it->deserialize; + } + } + } + + // Canary check: PROTOCOL_VERSION for endpoint 0, else json_crc. A mismatch + // means client and server disagree on the object model — ignore (per the + // spec the request gets no response; report through the error hook so the + // device application can see the client is desynced). + const uint16_t expected_canary = (endpoint_id == 0) ? kProtocolVersion : json_crc_snapshot; + if (trailer != expected_canary) { + if (on_error) + on_error("canary mismatch on endpoint " + std::to_string(endpoint_id) + + " (client/server JSON descriptors disagree); request ignored"); + return {}; + } + + std::vector data; + + if (endpoint_id == 0) { + // JSON blob read: payload is a u32 LE offset. + uint32_t offset = 0; + read_le(payload, offset); // leaves offset=0 if payload too short + const size_t len = json_snapshot.size(); + if (offset < len) { + const size_t chunk = std::min(output_len, 512); + const size_t end = std::min(len, static_cast(offset) + chunk); + data.assign(json_snapshot.begin() + offset, json_snapshot.begin() + end); + } + // offset >= len -> empty (terminates the client's read loop) + } else if (have_endpoint) { + // Property endpoint: write first (if payload present and writable), then + // read the current value into the response (if output_len > 0). + if (!payload.empty() && writable && deserialize) { + // The wire protocol carries no write status, so surface a failed / + // rejected write (bad payload size or setter refused) via the hook. + if (!deserialize(payload) && on_error) + on_error("write to endpoint " + std::to_string(endpoint_id) + " (" + ep_path + + ") failed; value not applied"); + } + if (output_len > 0 && serialize) { + std::vector value; + serialize(value); + // Cap the response to the smaller of what the client asked for + // (output_len), what the getter produced (value.size()), and the + // endpoint's declared wire width (ep_size). ep_size guards against a + // getter that (mis)serializes more than the endpoint's type advertises. + const size_t n = std::min({static_cast(output_len), value.size(), ep_size}); + data.assign(value.begin(), value.begin() + n); + } + } + // Unknown endpoint: ignore entirely per PROTOCOL.md -- return no response + // even if the client set the expect-response bit. (endpoint 0 is always a + // valid target; known write-only endpoints still get an empty-data ACK.) + if (endpoint_id != 0 && !have_endpoint) + return {}; + + if (!expect_response) + return {}; + + std::vector out; + append_u16_le(out, static_cast(seq_no | 0x8000)); + out.insert(out.end(), data.begin(), data.end()); + return out; + } + +private: + struct Endpoint { + uint16_t id{0}; + std::string path; // dotted, e.g. "axis0.controller.input_pos" + std::string type; // JSON primitive type name + size_t size{0}; // wire size in bytes + bool readable{false}; + bool writable{false}; + std::function &)> serialize; // append value LE + std::function)> deserialize; // read + apply + }; + + template + void register_typed(const std::string &path, const char *type_name, const getter_fn &getter, + const setter_fn &setter) { + // An endpoint with neither accessor cannot be read or written; registering + // it would misrepresent its access as "r" in the schema. Reject it. + if (!getter && !setter) + return; + std::scoped_lock lk(mutex_); + // ids >= 0x8000 collide with the expect-response bit and are unreachable + // (the dispatcher masks the endpoint field with 0x7fff) + if (next_id_ >= 0x8000) { + if (on_error_) + on_error_("endpoint id space exhausted (max 32767); '" + path + "' not registered"); + return; + } + Endpoint ep; + ep.id = next_id_++; + ep.path = path; + ep.type = type_name; + ep.size = sizeof(T); + ep.readable = static_cast(getter); + ep.writable = static_cast(setter); + if (getter) { + ep.serialize = [getter](std::vector &v) { append_le(v, getter()); }; + } + if (setter) { + ep.deserialize = [setter](std::span s) -> bool { + T val{}; + if (!read_le(s, val)) + return false; + std::error_code ec; + return setter(val, ec); + }; + } + endpoints_.push_back(std::move(ep)); + finalized_ = false; + } + + // ---- JSON descriptor generation (mutex_ held by caller) ---- + struct JsonNode { + std::string name; + bool is_property{false}; + // property fields: + uint16_t id{0}; + std::string type; + std::string access; + // object children (ordered by first registration): + std::vector children; + }; + + static JsonNode *find_child(JsonNode &parent, std::string_view name) { + auto it = std::find_if(parent.children.begin(), parent.children.end(), + [name](const JsonNode &c) { return c.name == name; }); + return it != parent.children.end() ? &*it : nullptr; + } + + static void append_entry(std::string &out, const JsonNode &node) { + out += "{\"name\":\""; + out += json_escape(node.name); + out += '"'; + if (node.is_property) { + out += ",\"id\":"; + out += std::to_string(node.id); + out += ",\"type\":\""; + out += node.type; + out += "\",\"access\":\""; + out += node.access; + out += "\"}"; + } else { + out += ",\"type\":\"object\",\"members\":"; + append_members(out, node); + out += '}'; + } + } + + static void append_members(std::string &out, const JsonNode &node) { + out += '['; + bool first = true; + for (const auto &c : node.children) { + if (!first) + out += ','; + first = false; + append_entry(out, c); + } + out += ']'; + } + + void finalize_locked() { + if (finalized_) + return; + JsonNode root; + for (const auto &ep : endpoints_) { + // split dotted path + std::vector parts; + size_t start = 0; + std::string_view p(ep.path); + while (true) { + size_t dot = p.find('.', start); + if (dot == std::string_view::npos) { + parts.push_back(p.substr(start)); + break; + } + parts.push_back(p.substr(start, dot - start)); + start = dot + 1; + } + JsonNode *cur = &root; + for (size_t i = 0; i + 1 < parts.size(); ++i) { + JsonNode *child = find_child(*cur, parts[i]); + if (!child) { + JsonNode obj; + obj.name = std::string(parts[i]); + obj.is_property = false; + cur->children.push_back(std::move(obj)); + child = &cur->children.back(); + } + cur = child; + } + JsonNode prop; + prop.name = std::string(parts.back()); + prop.is_property = true; + prop.id = ep.id; + prop.type = ep.type; + prop.access = ep.readable ? (ep.writable ? "rw" : "r") : (ep.writable ? "w" : "r"); + cur->children.push_back(std::move(prop)); + } + json_.clear(); + append_members(json_, root); + // The endpoint canary (interface-definition CRC) is CRC-16 over the JSON + // descriptor seeded with PROTOCOL_VERSION as the init value -- NOT the + // 0x1337 packet-CRC init. This matches the fw-v0.5.1 firmware + // json_crc_ = calc_crc16(PROTOCOL_VERSION, embedded_json, len) (endpoints_template.j2) + // and the reference fibre client + // json_crc16 = calc_crc16(PROTOCOL_VERSION, json_bytes) (discovery.py) + // so that a real fibre client's endpoint-N trailer matches. Verified by the + // serial-loopback interop harness (components/odrive_native/interop). + json_crc_ = odrive_crc16(json_, kProtocolVersion); + finalized_ = true; + } + + std::mutex mutex_; + std::vector endpoints_; + error_callback_fn on_error_{nullptr}; + uint16_t next_id_{1}; // 0 reserved for JSON blob + bool finalized_{false}; + std::string json_; + uint16_t json_crc_{0}; +}; + +} // namespace detail +} // namespace espp diff --git a/components/odrive_native/include/detail/odrive_native_stream.hpp b/components/odrive_native/include/detail/odrive_native_stream.hpp new file mode 100644 index 000000000..c5691782c --- /dev/null +++ b/components/odrive_native/include/detail/odrive_native_stream.hpp @@ -0,0 +1,157 @@ +#pragma once + +// ODrive legacy native (Fibre) UART *stream* framing. +// +// The packet codec lives in detail/odrive_native_core.hpp. Over USB, each bulk +// transfer carries exactly one packet and USB provides framing + reliability. +// Over a UART/serial link there is no such structure, so fibre's serial backend +// wraps every packet in a small stream frame with two CRCs: +// +// [0xAA sync] +// [len u8 ] packet length, MUST be < 128 +// [crc8 u8 ] CRC8 over the two bytes [sync,len], init 0x42, poly 0x37 +// [packet len ] the raw packet bytes (see odrive_native_core.hpp) +// [crc16 u16 BE] CRC16 over the packet bytes, init 0x1337, poly 0x3d65, +// transmitted big-endian (high byte first) +// +// Receiver validation trick (holds for these CRCs, and is what fibre relies on): +// * CRC8 over [sync,len,crc8] == 0 +// * CRC16 over [packet .. crc16 bytes] == 0 +// +// This header is host-buildable with nothing but a C++20 standard library and the +// core header (for odrive_crc16). It has zero ESP-IDF / FreeRTOS dependencies so +// the interop device shim and golden tests build with a plain `c++ -std=c++20`. + +#include +#include +#include +#include + +#include "detail/odrive_native_core.hpp" + +namespace espp { +namespace detail { + +/// The stream sync byte that begins every frame. +static constexpr uint8_t kStreamSync = 0xAA; +/// The stream framing caps a single packet at 127 bytes (len must be < 128). +static constexpr size_t kStreamMaxPacket = 127; + +/// ODrive/fibre stream CRC8 (poly 0x37, init 0x42, non-reflected, MSB-first). +/// Fold a single byte through the running remainder. +inline uint8_t odrive_crc8_byte(uint8_t rem, uint8_t val) { + rem ^= val; + for (int i = 0; i < 8; i++) + rem = (rem & 0x80) ? static_cast((rem << 1) ^ 0x37) : static_cast(rem << 1); + return rem; +} + +/// CRC8 over a buffer using the fibre stream init value (0x42). +inline uint8_t odrive_crc8(std::span data, uint8_t init = 0x42) { + return std::accumulate(data.begin(), data.end(), init, odrive_crc8_byte); +} + +/** + * @brief Wrap one packet in a fibre serial stream frame. + * @param packet The raw packet bytes (<= kStreamMaxPacket). The caller is + * responsible for ensuring the packet fits; e.g. the endpoint-0 JSON read + * response must be truncated so the packet is <= 127 bytes. + * @return The framed byte stream: sync, len, crc8, packet, crc16(BE). + */ +inline std::vector stream_frame(std::span packet) { + std::vector out; + // The stream framing carries the packet length in a single byte that must be + // < 128. A larger packet cannot be represented (len would wrap/truncate and + // produce a malformed frame), so refuse it and return an empty vector. + if (packet.size() > kStreamMaxPacket) + return out; + const uint8_t len = static_cast(packet.size()); + out.reserve(packet.size() + 5); + out.push_back(kStreamSync); + out.push_back(len); + const uint8_t header[2] = {kStreamSync, len}; + out.push_back(odrive_crc8(std::span(header, 2))); + out.insert(out.end(), packet.begin(), packet.end()); + const uint16_t c = odrive_crc16(packet); + out.push_back(static_cast((c >> 8) & 0xff)); // big-endian: high byte + out.push_back(static_cast(c & 0xff)); // low byte + return out; +} + +/** + * @brief Stateful deframer for the fibre serial stream. + * + * Feed arbitrary chunks of received stream bytes with push(); it buffers partial + * input, resynchronizes on the 0xAA sync byte, validates the CRC8 header and the + * CRC16 trailer, and returns each complete, verified packet. A frame that fails + * either CRC (or carries len >= 128) is discarded and the deframer resynchronizes + * at the next 0xAA. + */ +class StreamDeframer { +public: + /// Append received stream bytes and return any complete packets decoded. + std::vector> push(std::span data) { + buf_.insert(buf_.end(), data.begin(), data.end()); + std::vector> out; + // `pos_` is a read cursor into buf_. Resync/consume advance the cursor + // instead of erasing from the front of the vector, which would be O(n) per + // byte dropped (and quadratic under noisy input / repeated resync). We + // compact the vector once at the end, so a full push() is O(buffer size). + for (;;) { + // Resync: advance past everything before the first sync byte. + while (pos_ < buf_.size() && buf_[pos_] != kStreamSync) + ++pos_; + + // Need at least the 3-byte header [sync,len,crc8]. + // (Not always true: the resync loop above can also exit with pos_ at a + // sync byte 1-2 bytes before the end of the buffer.) + // cppcheck-suppress knownConditionTrueFalse + if (buf_.size() - pos_ < 3) + break; + + const uint8_t len = buf_[pos_ + 1]; + const uint8_t hcrc = buf_[pos_ + 2]; + const uint8_t header[2] = {buf_[pos_], len}; + if (len >= 128 || odrive_crc8(std::span(header, 2)) != hcrc) { + // Bad header: skip the sync byte and hunt for the next one. + ++pos_; + continue; + } + + // Need the full frame: header(3) + packet(len) + crc16(2). + const size_t frame_len = 3 + static_cast(len) + 2; + if (buf_.size() - pos_ < frame_len) + break; // wait for more bytes + + std::span packet(buf_.data() + pos_ + 3, len); + const uint16_t got = + static_cast((static_cast(buf_[pos_ + 3 + len]) << 8) | + buf_[pos_ + 3 + len + 1]); // big-endian + if (odrive_crc16(packet) != got) { + // Bad trailer CRC: skip the sync byte and resync. + ++pos_; + continue; + } + + out.emplace_back(packet.begin(), packet.end()); + pos_ += frame_len; + } + // Compact: drop the consumed prefix in a single erase, then reset the + // cursor. This is the only front-erase per push() (amortized O(1) per byte). + if (pos_ > 0) { + buf_.erase(buf_.begin(), buf_.begin() + pos_); + pos_ = 0; + } + return out; + } + + /// Bytes currently buffered awaiting a complete frame (for diagnostics/tests). + size_t buffered() const { return buf_.size() - pos_; } + +private: + std::vector buf_; + size_t pos_ = 0; // read cursor into buf_ (bytes before it are consumed) +}; + +} // namespace detail +} // namespace espp diff --git a/components/odrive_native/include/odrive_native.hpp b/components/odrive_native/include/odrive_native.hpp new file mode 100644 index 000000000..3c88592df --- /dev/null +++ b/components/odrive_native/include/odrive_native.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "detail/odrive_native_core.hpp" + +namespace espp { + +/** + * @brief ODrive legacy native (Fibre endpoint) binary protocol server. + * + * Implements the packet-based ODrive legacy endpoint protocol (fw <= 0.5.x) as + * used over the USB vendor interface, where each USB bulk transfer carries + * exactly one packet. The component is transport-agnostic and performs no I/O + * itself: feed one inbound request packet to process_bytes() and transmit the + * returned response packet (empty when no response is expected). + * + * Applications register typed properties from dotted paths (mirroring + * espp::OdriveAscii). Endpoint ids are assigned sequentially starting at 1 + * (endpoint 0 is reserved for the JSON descriptor blob), and the compact JSON + * descriptor plus its CRC are finalized lazily on first use. This lets a legacy + * odrivetool / fibre-python client auto-discover the object tree and perform + * typed get/set. + * + * The registration API and dispatch are provided by espp::detail:: + * OdriveNativeCore, a host-buildable wire core with no ESP dependencies; this + * class adds the espp logging identity via BaseComponent. + * + * See PROTOCOL.md for the authoritative wire specification. + * + * \section odrive_native_ex1 Basic Example + * \snippet odrive_native_example.cpp odrive_native_basic_example + */ +class OdriveNative : public BaseComponent, public detail::OdriveNativeCore { +public: + /** + * @brief Configuration for the OdriveNative server. + */ + struct Config { + espp::Logger::Verbosity log_level{espp::Logger::Verbosity::WARN}; /**< Logger verbosity. */ + }; + + /** + * @brief Create an OdriveNative protocol server. + * @param config Configuration parameters. + */ + explicit OdriveNative(const Config &config) + : BaseComponent("ODriveNative", config.log_level) { + // The wire protocol has no error channel; route the core's dropped-request / + // failed-write reports through the component logger so they are observable. + set_error_callback([this](const std::string &msg) { logger_.warn("{}", msg); }); + } + + OdriveNative() + : OdriveNative(Config{}) {} +}; + +} // namespace espp diff --git a/components/odrive_native/interop/.gitignore b/components/odrive_native/interop/.gitignore new file mode 100644 index 000000000..243530a4d --- /dev/null +++ b/components/odrive_native/interop/.gitignore @@ -0,0 +1,3 @@ +# Interop runtime artifacts -- fetched/created by run_interop.sh, never committed. +.venv-odrive/ +odrive-ref/ diff --git a/components/odrive_native/interop/README.md b/components/odrive_native/interop/README.md new file mode 100644 index 000000000..af12f9a96 --- /dev/null +++ b/components/odrive_native/interop/README.md @@ -0,0 +1,44 @@ +# ODrive native — real fibre serial-loopback interop + +The **real-tool gate** for `components/odrive_native`, mirroring how +`components/rtps` is gated against real FastDDS / ROS 2. A **genuine reference +fibre client** — the pure-python legacy `fibre` shipped in +`odriverobotics/ODrive` @ **`fw-v0.5.1`** (`Firmware/fibre/python/fibre`), the +exact implementation the espp wire codec was written against — connects to a host +build of the `odrive_native` device shim over a **PTY serial loopback**, downloads +endpoint 0, enumerates the object tree, and reads/writes endpoints. + +## Pieces +- `odrive_native_interop_device.cpp` — host device shim. Opens a PTY (or a serial + path arg), registers a small ODrive-like tree on an `OdriveNativeCore`, and runs + the serve loop: stream bytes → `StreamDeframer` → `process_bytes` → `stream_frame` + → write. Uses only the host-buildable `detail/` headers (plain `c++ -std=c++20`, + no espp lib). Prints `PTY_SLAVE ` on startup. +- `odrive_fibre_client.py` — drives the real reference fibre library + (`find_any("serial:")`): connect, enumerate the tree, read a value, + write-then-read-back a value, assert. Exit 0 on success. +- `run_interop.sh` — builds the golden host tests + device shim, runs the goldens, + fetches the reference client (sparse clone + venv with `pyserial`+`appdirs`), + spawns the shim on a PTY, runs the real client. Prints `RESULT PASS/FAIL: ` + and exits non-zero on any failure. +- `run.sh` — thin host entry (`exec run_interop.sh`); no Docker needed. + +## Run locally +```sh +cd components/odrive_native/interop +./run.sh +``` +Reuses an existing `odrive-ref/` clone and `.venv-odrive/` on reruns (both +git-ignored). Override the interpreter with `PYTHON=python3.x ./run.sh`. + +## CI +`.github/workflows/odrive_native_interop.yml` runs `run.sh` on `ubuntu-latest` +(Python 3.11), gated PASS/FAIL, triggered by `components/odrive_native/**`, +`pc/tests/odrive_native_*`, and the workflow file. + +## Wire note (found by this harness) +The endpoint **canary** (`json_crc`) is `calc_crc16(json_bytes, init=PROTOCOL_VERSION=1)`, +**not** the 0x1337 packet-CRC init. This matches the fw-v0.5.1 firmware +(`endpoints_template.j2`) and the reference client (`discovery.py`). Only the UART +*stream* framing CRC16 and endpoint 0's packet trailer use 0x1337 / PROTOCOL_VERSION +respectively. The core was corrected accordingly; see `PROTOCOL.md`. diff --git a/components/odrive_native/interop/odrive_fibre_client.py b/components/odrive_native/interop/odrive_fibre_client.py new file mode 100755 index 000000000..b0ce1f56e --- /dev/null +++ b/components/odrive_native/interop/odrive_fibre_client.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +"""Real-tool interop CLIENT for the espp odrive_native device shim. + +This drives the GENUINE legacy pure-python ``fibre`` library shipped in the ODrive +firmware (``odriverobotics/ODrive`` @ ``fw-v0.5.1``, +``Firmware/fibre/python/fibre``) -- the exact reference implementation the espp +``OdriveNativeCore`` wire codec was built against. It connects to the device shim +over a serial port / PTY, downloads endpoint 0, enumerates the object tree, reads a +value, and writes-then-reads-back a value, asserting each step. + +Exit 0 on success, non-zero + diagnostics on failure. + +Usage: + odrive_fibre_client.py [--fibre-path DIR] [--timeout SECONDS] +""" +import argparse +import sys +import time + + +def log(msg): + print("[client] " + msg, flush=True) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("port", help="serial port / PTY slave path the device shim printed") + ap.add_argument("--fibre-path", default=None, + help="path to Firmware/fibre/python (the legacy fibre package)") + ap.add_argument("--timeout", type=float, default=15.0) + args = ap.parse_args() + + if args.fibre_path: + sys.path.insert(0, args.fibre_path) + + try: + import fibre # noqa: F401 + from fibre import find_any + from fibre.utils import Logger + except Exception as e: # pragma: no cover - environment issue + log("FAILED to import the reference fibre library: %r" % e) + log("Provide it with --fibre-path /Firmware/fibre/python") + return 3 + + log("fibre reference library: %s" % fibre.__file__) + # A serial: path spec makes fibre's serial backend scan for a port whose name + # matches the (regex-anchored) path -- our PTY slave, e.g. /dev/ttys011. + path_spec = "serial:" + args.port + log("connecting via find_any(path=%r, timeout=%ss)..." % (path_spec, args.timeout)) + + dev = find_any(path=path_spec, timeout=args.timeout, logger=Logger(verbose=False)) + if dev is None: + log("FAILED: no device discovered on %s within %ss" % (args.port, args.timeout)) + return 1 + + log("CONNECTED. Enumerating endpoint tree downloaded from endpoint 0:") + + # Walk the endpoint-0 JSON member tree that fibre downloaded + parsed. + def walk(members, prefix=""): + names = [] + for m in members: + name = m.get("name") + full = (prefix + "." + name) if prefix else name + if m.get("type") == "object": + names.append((full, "object", None)) + names.extend(walk(m.get("members", []), full)) + else: + names.append((full, m.get("type"), m.get("access", ""))) + return names + + tree = walk(dev.__dict__.get("_json_data", [])) + for full, typ, access in tree: + if typ == "object": + log(" %-40s (object)" % full) + else: + log(" %-40s %-8s %s" % (full, typ, access)) + + props = {full for (full, typ, _a) in tree if typ != "object"} + required = { + "vbus_voltage", + "axis0.error", + "axis0.controller.input_pos", + "axis0.controller.config.vel_limit", + "serial_number", + } + missing = required - props + if missing: + log("FAILED: endpoint tree is missing %s" % sorted(missing)) + return 1 + log("endpoint tree contains all %d expected properties" % len(required)) + + def get(path): + obj = dev + parts = path.split(".") + for p in parts[:-1]: + obj = getattr(obj, p) + return getattr(obj, parts[-1]) + + def set_(path, value): + obj = dev + parts = path.split(".") + for p in parts[:-1]: + obj = getattr(obj, p) + setattr(obj, parts[-1], value) + + # 1) Read a value. + vbus = get("vbus_voltage") + log("READ vbus_voltage = %r" % vbus) + if abs(vbus - 24.37) > 1e-3: + log("FAILED: vbus_voltage expected ~24.37, got %r" % vbus) + return 1 + + sn = get("serial_number") + log("READ serial_number = 0x%X" % sn) + if sn != 0x00A1B2C3D4E5: + log("FAILED: serial_number mismatch, got 0x%X" % sn) + return 1 + + err = get("axis0.error") + log("READ axis0.error = %r" % err) + + # 2) Write then read back a value. + new_pos = 3.14159 + log("WRITE axis0.controller.input_pos <- %r" % new_pos) + set_("axis0.controller.input_pos", new_pos) + time.sleep(0.1) + readback = get("axis0.controller.input_pos") + log("READ axis0.controller.input_pos = %r" % readback) + if abs(readback - new_pos) > 1e-4: + log("FAILED: input_pos read-back %r != written %r" % (readback, new_pos)) + return 1 + + # 3) Write-then-read a second rw property to be thorough. + new_vlim = 42.5 + log("WRITE axis0.controller.config.vel_limit <- %r" % new_vlim) + set_("axis0.controller.config.vel_limit", new_vlim) + time.sleep(0.1) + vlim = get("axis0.controller.config.vel_limit") + log("READ axis0.controller.config.vel_limit = %r" % vlim) + if abs(vlim - new_vlim) > 1e-4: + log("FAILED: vel_limit read-back %r != written %r" % (vlim, new_vlim)) + return 1 + + log("ALL INTEROP ASSERTIONS PASSED (real fibre client <-> espp device)") + return 0 + + +if __name__ == "__main__": + try: + rc = main() + except Exception: + import traceback + traceback.print_exc() + rc = 2 + sys.exit(rc) diff --git a/components/odrive_native/interop/odrive_native_interop_device.cpp b/components/odrive_native/interop/odrive_native_interop_device.cpp new file mode 100644 index 000000000..d90eb1bfc --- /dev/null +++ b/components/odrive_native/interop/odrive_native_interop_device.cpp @@ -0,0 +1,196 @@ +// ODrive legacy native (Fibre) interop DEVICE shim. +// +// Emulates an ODrive over a serial link so a REAL fibre client (the pure-python +// legacy fibre from odriverobotics/ODrive @ fw-v0.5.1) can connect, enumerate the +// endpoint tree, and read/write endpoints -- the true real-tool interop gate, +// mirroring how components/rtps is gated against FastDDS / ROS 2. +// +// It uses ONLY the host-buildable detail/ headers (OdriveNativeCore for the packet +// codec + JSON descriptor, StreamDeframer/stream_frame for the UART framing) so it +// builds with a plain `c++ -std=c++20` -- no BaseComponent, no espp lib link. +// +// Transport: opens a pseudo-terminal (posix_openpt/grantpt/unlockpt/ptsname) and +// prints the slave path, OR uses a serial device path given as argv[1]. The read +// loop is: raw stream bytes -> StreamDeframer -> OdriveNativeCore::process_bytes +// -> stream_frame -> write back. Endpoint-0 (JSON) responses are truncated so the +// framed packet stays <= 127 bytes (the stream cap); the client's chunked read +// loop advances by the bytes it actually receives, so truncation is safe. +// +// Build: c++ -std=c++20 -I../include odrive_native_interop_device.cpp -o device +// Run: ./device # opens a PTY, prints the slave path +// ./device /dev/ttyS0 # uses an existing serial device + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "detail/odrive_native_core.hpp" +#include "detail/odrive_native_stream.hpp" + +using espp::detail::kStreamMaxPacket; +using espp::detail::OdriveNativeCore; +using espp::detail::stream_frame; +using espp::detail::StreamDeframer; + +namespace { + +// Put a tty/pty into raw mode so the fibre binary stream passes through untouched +// (no CR/LF translation, no XON/XOFF flow control eating 0x11/0x13, no signal +// chars). Applied to either PTY end configures the shared line discipline. +void make_raw(int fd) { + struct termios t; + if (tcgetattr(fd, &t) != 0) + return; + cfmakeraw(&t); + t.c_cc[VMIN] = 1; // block for at least 1 byte + t.c_cc[VTIME] = 0; // no inter-byte timer + tcsetattr(fd, TCSANOW, &t); +} + +} // namespace + +int main(int argc, char **argv) { + // ---- Build a small demo ODrive-like endpoint tree ---------------------- + OdriveNativeCore core; + + float vbus = 24.37f; + uint32_t axis0_error = 0; + float input_pos = 0.0f; + float vel_limit = 20.0f; + uint64_t serial_number = 0x00A1B2C3D4E5ULL; + + core.register_float_property("vbus_voltage", [&] { return vbus; }); + core.register_uint32_property("axis0.error", [&] { return axis0_error; }); + core.register_float_property( + "axis0.controller.input_pos", [&] { return input_pos; }, + [&](float v, std::error_code &ec) { + input_pos = v; + ec.clear(); + return true; + }); + core.register_float_property( + "axis0.controller.config.vel_limit", [&] { return vel_limit; }, + [&](float v, std::error_code &ec) { + vel_limit = v; + ec.clear(); + return true; + }); + core.register_uint64_property("serial_number", [&] { return serial_number; }); + core.finalize(); + + std::fprintf(stderr, "[device] JSON descriptor (%zu bytes, crc=0x%04x): %s\n", core.json().size(), + core.json_crc(), core.json().c_str()); + + // ---- Open the transport (PTY or a given serial path) ------------------- + int fd = -1; + if (argc > 1) { + fd = ::open(argv[1], O_RDWR | O_NOCTTY); + if (fd < 0) { + std::fprintf(stderr, "[device] failed to open %s: %s\n", argv[1], std::strerror(errno)); + return 1; + } + make_raw(fd); + std::fprintf(stderr, "[device] using serial device %s\n", argv[1]); + } else { + fd = ::posix_openpt(O_RDWR | O_NOCTTY); + if (fd < 0 || ::grantpt(fd) != 0 || ::unlockpt(fd) != 0) { + std::fprintf(stderr, "[device] failed to open PTY master: %s\n", std::strerror(errno)); + return 1; + } + make_raw(fd); + const char *slave = ::ptsname(fd); + if (!slave) { + std::fprintf(stderr, "[device] ptsname failed: %s\n", std::strerror(errno)); + return 1; + } + // The runner parses this exact line to learn the port to hand the client. + std::printf("PTY_SLAVE %s\n", slave); + std::fflush(stdout); + std::fprintf(stderr, "[device] PTY slave = %s\n", slave); + } + + // ---- Serve: stream bytes -> deframe -> process -> reframe -> write ------ + StreamDeframer deframer; + uint8_t rx[512]; + // Self-terminate after a stretch of inactivity so a crashed client never + // leaves the shim running forever; the runner also kills it explicitly. + const int kIdleTimeoutSec = 30; + time_t last_activity = ::time(nullptr); + + for (;;) { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + struct timeval tv { + 1, 0 + }; + int sel = ::select(fd + 1, &rfds, nullptr, nullptr, &tv); + if (sel < 0) { + if (errno == EINTR) + continue; + break; + } + if (sel == 0) { + if (::time(nullptr) - last_activity > kIdleTimeoutSec) { + std::fprintf(stderr, "[device] idle timeout, exiting\n"); + break; + } + continue; + } + + ssize_t n = ::read(fd, rx, sizeof(rx)); + if (n < 0) { + // EIO happens on a PTY when the slave side is (re)opened/closed; tolerate. + if (errno == EIO || errno == EAGAIN || errno == EINTR) { + usleep(2000); + continue; + } + break; + } + if (n == 0) { + usleep(2000); + continue; + } + last_activity = ::time(nullptr); + + auto packets = deframer.push(std::span(rx, static_cast(n))); + for (auto &pkt : packets) { + std::vector resp = core.process_bytes(pkt); + if (resp.empty()) + continue; // fire-and-forget request, no ACK expected + // Stream cap: keep the framed packet <= 127 bytes. Only the endpoint-0 + // JSON chunk can exceed this; truncating it is safe (client re-reads by + // offset). The 2-byte response seq header is always preserved. + if (resp.size() > kStreamMaxPacket) + resp.resize(kStreamMaxPacket); + auto framed = stream_frame(resp); + ssize_t off = 0; + while (off < static_cast(framed.size())) { + ssize_t w = ::write(fd, framed.data() + off, framed.size() - off); + if (w < 0) { + if (errno == EINTR || errno == EAGAIN) { + usleep(1000); + continue; + } + break; + } + off += w; + } + } + } + + ::close(fd); + return 0; +} diff --git a/components/odrive_native/interop/run.sh b/components/odrive_native/interop/run.sh new file mode 100755 index 000000000..8fc81dbc5 --- /dev/null +++ b/components/odrive_native/interop/run.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +# Host-side entry point for the ODrive native serial-loopback interop test. +# Usage: ./run.sh (from components/odrive_native/interop) +# +# Unlike the rtps interop (which needs a ROS 2 / FastDDS container), this test +# needs only a C++20 compiler, python3, and network access to fetch the reference +# fibre client, so it runs directly on the host -- no Docker required. +set -euo pipefail +cd "$(dirname "$0")" +exec bash ./run_interop.sh diff --git a/components/odrive_native/interop/run_interop.sh b/components/odrive_native/interop/run_interop.sh new file mode 100755 index 000000000..7a13241f0 --- /dev/null +++ b/components/odrive_native/interop/run_interop.sh @@ -0,0 +1,161 @@ +#!/usr/bin/env bash +# ODrive legacy native (Fibre) serial-loopback interop test. +# +# The true real-tool gate for components/odrive_native, mirroring how +# components/rtps is gated against real FastDDS / ROS 2: a GENUINE reference fibre +# client (the pure-python legacy fibre from odriverobotics/ODrive @ fw-v0.5.1) +# connects to the host build of the odrive_native device shim over a PTY serial +# loopback, downloads endpoint 0, enumerates the endpoint tree, and reads/writes +# endpoints. +# +# Steps: +# 1. Build the golden host tests + the device shim with a plain c++ (no espp lib). +# 2. Run the golden wire-format tests (CRC8/CRC16 + frame bytes + round-trips). +# 3. Fetch the reference fibre client (shallow clone) + a venv with pyserial. +# 4. Spawn the device shim on a PTY, run the real client against it. +# +# Prints `RESULT PASS: ` / `RESULT FAIL: ` lines; exits non-zero on any +# failure. Reuses an existing clone/venv if present (fast local reruns). +set -uo pipefail + +cd "$(dirname "$0")" +INTEROP_DIR="$(pwd)" +COMPONENT_DIR="$(cd .. && pwd)" +INC="$COMPONENT_DIR/include" +WORK="${TMPDIR:-/tmp}/odrive_native_interop" +mkdir -p "$WORK" + +PASS=0 +FAIL=0 +note() { echo -e "\n===== $* ====="; } +result() { # name exit_code + if [ "$2" -eq 0 ]; then echo "RESULT PASS: $1"; PASS=$((PASS + 1)); + else echo "RESULT FAIL: $1"; FAIL=$((FAIL + 1)); fi +} + +CXX="${CXX:-c++}" + +# --- 1. Build golden tests + device shim ------------------------------------ +note "Build golden host tests + device shim ($CXX -std=c++20)" +build_rc=0 +"$CXX" -std=c++20 -I"$INC" "$COMPONENT_DIR/test/odrive_native_host_test.cpp" \ + -o "$WORK/host_test" || build_rc=1 +"$CXX" -std=c++20 -I"$INC" "$COMPONENT_DIR/test/odrive_native_stream_test.cpp" \ + -o "$WORK/stream_test" || build_rc=1 +"$CXX" -std=c++20 -I"$INC" "$INTEROP_DIR/../../../pc/tests/odrive_native_golden.cpp" \ + -o "$WORK/golden" || build_rc=1 +"$CXX" -std=c++20 -I"$INC" "$INTEROP_DIR/odrive_native_interop_device.cpp" \ + -o "$WORK/device" || build_rc=1 +result "build" $build_rc +if [ $build_rc -ne 0 ]; then + echo "INTEROP FAIL"; exit 1 +fi + +# --- 2. Golden wire-format tests (no external tool) ------------------------- +note "Golden wire-format tests (packet codec)" +"$WORK/host_test"; result "packet_golden" $? +note "Golden wire-format tests (stream framing)" +"$WORK/stream_test"; result "stream_golden" $? +note "Golden wire-format tests (combined pc golden)" +"$WORK/golden"; result "wire_golden" $? + +# --- 3. Reference fibre client (real-tool) ---------------------------------- +note "Set up the reference fibre client (odriverobotics/ODrive @ fw-v0.5.1)" +REF_DIR="$INTEROP_DIR/odrive-ref" +FIBRE_PY="$REF_DIR/Firmware/fibre/python" +if [ ! -d "$FIBRE_PY/fibre" ]; then + echo "cloning ODrive fw-v0.5.1 (sparse: Firmware/fibre/python only)..." + rm -rf "$REF_DIR" + git clone --depth 1 --branch fw-v0.5.1 --filter=blob:none --sparse \ + https://github.com/odriverobotics/ODrive.git "$REF_DIR" \ + && git -C "$REF_DIR" sparse-checkout set Firmware/fibre/python +fi +if [ ! -d "$FIBRE_PY/fibre" ]; then + echo "reference fibre client unavailable (clone failed)"; result "fibre_client_setup" 1 + echo ""; echo "PASS=$PASS FAIL=$FAIL"; echo "INTEROP FAIL"; exit 1 +fi +result "fibre_client_setup" 0 + +VENV="$INTEROP_DIR/.venv-odrive" +PYBIN="$VENV/bin/python" +if [ ! -x "$PYBIN" ]; then + PY="${PYTHON:-python3}" + echo "creating venv with $PY and installing pyserial+appdirs..." + "$PY" -m venv "$VENV" \ + && "$PYBIN" -m pip install --quiet --upgrade pip \ + && "$PYBIN" -m pip install --quiet pyserial appdirs +fi +"$PYBIN" -c "import serial, appdirs" 2>/dev/null +venv_rc=$? +result "venv_deps" $venv_rc +if [ $venv_rc -ne 0 ]; then + echo ""; echo "PASS=$PASS FAIL=$FAIL"; echo "INTEROP FAIL"; exit 1 +fi + +# --- 4. Spawn the device shim on a PTY, run the real client ----------------- +note "Real fibre client <-> espp device shim (PTY serial loopback)" +DEV_OUT="$WORK/device.out" +DEV_ERR="$WORK/device.err" +: > "$DEV_OUT" +"$WORK/device" > "$DEV_OUT" 2> "$DEV_ERR" & +DEVPID=$! + +PTY="" +for _ in $(seq 1 100); do + PTY=$(grep -oE 'PTY_SLAVE .*' "$DEV_OUT" 2>/dev/null | awk '{print $2}') + [ -n "$PTY" ] && break + # bail early if the device died + kill -0 "$DEVPID" 2>/dev/null || break + sleep 0.1 +done + +if [ -z "$PTY" ]; then + echo "device shim did not report a PTY slave"; cat "$DEV_ERR" + kill "$DEVPID" 2>/dev/null + result "real_fibre_interop" 1 +else + echo "device PTY slave = $PTY" + # The reference fibre client enumerates candidate ports with a plain + # os.listdir('/dev') (top-level entries only) + pyserial's comports(). On + # macOS a PTY slave is a top-level node (/dev/ttysNNN) and is found; on + # Linux it is nested (/dev/pts/N), which that enumeration can never see, so + # discovery would always time out. Alias the slave to a top-level /dev + # symlink so the UNMODIFIED reference client can discover it -- this works + # around only the client's port-scan quirk, not anything on the wire. + CLIENT_PORT="$PTY" + PTY_LINK="" + case "$PTY" in + /dev/pts/*) + PTY_LINK="/dev/fibre-interop-$$" + SUDO="" + [ "$(id -u)" -ne 0 ] && command -v sudo >/dev/null 2>&1 && SUDO="sudo" + if $SUDO ln -sf "$PTY" "$PTY_LINK" 2>/dev/null; then + CLIENT_PORT="$PTY_LINK" + echo "aliased $PTY -> $PTY_LINK (Linux: fibre's port scan only sees top-level /dev entries)" + else + echo "WARNING: could not create $PTY_LINK; the reference client cannot" + echo " discover nested /dev/pts/* slaves and will likely time out" + PTY_LINK="" + fi + ;; + esac + echo "--- device JSON descriptor ---"; sed -n 's/^\[device\] //p' "$DEV_ERR" | head -1 + "$PYBIN" "$INTEROP_DIR/odrive_fibre_client.py" "$CLIENT_PORT" \ + --fibre-path "$FIBRE_PY" --timeout 20 + client_rc=$? + kill "$DEVPID" 2>/dev/null + wait "$DEVPID" 2>/dev/null + if [ -n "$PTY_LINK" ]; then + SUDO="" + [ "$(id -u)" -ne 0 ] && command -v sudo >/dev/null 2>&1 && SUDO="sudo" + $SUDO rm -f "$PTY_LINK" 2>/dev/null || true + fi + result "real_fibre_interop" $client_rc +fi + +# --- Summary ---------------------------------------------------------------- +echo "" +echo "==================== SUMMARY ====================" +echo "PASS=$PASS FAIL=$FAIL" +if [ $FAIL -eq 0 ]; then echo "INTEROP PASS"; else echo "INTEROP FAIL"; fi +exit $FAIL diff --git a/components/odrive_native/python/.gitignore b/components/odrive_native/python/.gitignore new file mode 100644 index 000000000..77ac75498 --- /dev/null +++ b/components/odrive_native/python/.gitignore @@ -0,0 +1,3 @@ +.venv/ +__pycache__/ +*.pyc diff --git a/components/odrive_native/python/README.md b/components/odrive_native/python/README.md new file mode 100644 index 000000000..b020e2a7a --- /dev/null +++ b/components/odrive_native/python/README.md @@ -0,0 +1,130 @@ +# espp_odrive — Python client for the ODrive native (Fibre endpoint) protocol + +A clean, ergonomic, **odrivetool-equivalent** Python client for the ODrive +legacy **native** (Fibre endpoint) binary protocol — a from-scratch +re-implementation of the documented wire format in +[`../PROTOCOL.md`](../PROTOCOL.md). + +It depends only on the Python **standard library** plus **`pyserial`** (for the +serial backend, imported lazily). There is **no dependency on the +`odrive`/`fibre` pip package** — that is the whole point: you fully own this +code. + +> **Shipped with the espp Python package**: this directory is the single source +> of truth, and the [espp wheel](https://pypi.org/project/espp/) ships it as the +> top-level `espp_odrive` package (also reachable as `espp.odrive`) — so +> `pip install espp[serial]` gives you both the bound C++ protocol **server** +> (`espp.OdriveNative`) and this **client**, with no path tricks. See +> `python/odrive_native_test.py` at the repo root, which loops the two against +> each other in-process. + +## Install / requirements + +- Python 3.8+ +- `pyserial` (only needed for the serial transport) + +```bash +python3 -m venv .venv && .venv/bin/pip install pyserial +``` + +## Usage + +```python +from espp_odrive import connect + +dev = connect("/dev/ttyUSB0") # downloads endpoint-0 JSON, builds the tree + +print("vbus:", dev.vbus_voltage) # typed read (float) +print("serial: 0x%X" % dev.serial_number) + +dev.axis0.controller.input_pos = 3.14 # typed write (nested objects as attributes) +print(dev.axis0.controller.input_pos) # read back + +# Dotted-path helpers: +dev.set("axis0.controller.config.vel_limit", 42.5) +print(dev.get("axis0.controller.config.vel_limit")) + +dev.dump() # pretty-print the whole tree with live values +dev.close() +``` + +`find()` scans available serial ports and returns the first ODrive that answers +(or `None`): + +```python +from espp_odrive import find +dev = find(timeout=5.0) +``` + +### ASCII protocol (optional, separate) + +A thin, self-contained helper for the ODrive **ASCII** protocol (`r/w/p/v/f` +text lines) — unrelated to the binary native protocol above: + +```python +from espp_odrive import OdriveAscii +a = OdriveAscii("/dev/ttyUSB0") +a.position(0, 3.14) # p 0 3.14 0 0 +pos, vel = a.feedback(0) # f 0 +vbus = float(a.read("vbus_voltage")) +``` + +## Package layout + +| File | Responsibility | +|------|----------------| +| `espp_odrive/crc.py` | CRC8 / CRC16 (non-reflected, MSB-first) + constants | +| `espp_odrive/protocol.py` | Packet build/parse, little-endian type codecs | +| `espp_odrive/transport.py` | `Transport` interface + serial **stream framing** backend (`SerialStreamTransport`, `StreamDeframer`, `stream_frame`) | +| `espp_odrive/device.py` | `Channel` (seq + request/response), object tree (`RemoteObject`/`RemoteProperty`), `connect`/`find`/`Device` | +| `espp_odrive/ascii.py` | Optional minimal ASCII-protocol helper | + +### Transports (serial now, USB later) + +The stack talks to the device through a `Transport`, which moves whole +**packets**: + +```python +class Transport(ABC): + def send_packet(self, packet: bytes) -> None: ... + def read_packet(self, timeout: float) -> bytes | None: ... +``` + +- `SerialStreamTransport` implements the UART **stream framing** + (`[0xAA][len][crc8][packet][crc16 big-endian]`) with resynchronizing + deframing, since a raw serial line has no packet structure. +- **USB seam:** over USB each bulk transfer already *is* one packet, so a future + USB backend just implements `send_packet`/`read_packet` with no framing and is + passed to `connect_transport(transport)`. Nothing above the transport changes. + +## Wire notes (implemented exactly) + +- **CRC16**: poly `0x3d65`, non-reflected MSB-first. Init `0x1337` for stream + framing and the endpoint-0 packet trailer; init **`PROTOCOL_VERSION=1`** for + the endpoint `json_crc` canary. Golden: `crc16("123456789", 0x1337)=0xaa01`. +- **CRC8** (stream header only): poly `0x37`, init `0x42`. +- **Packet** (LE): `[seq u16][endpoint u16 (bit15=expect response)][output_len u16][payload][trailer u16]`; + response `[seq|0x8000 u16][data...]`. Endpoint 0 = chunked JSON read (payload = + u32 LE offset; empty response when offset ≥ len). +- Sequence numbers increment mod `0x7fff` with bit `0x80` hardwired to 1 (to + avoid clashing with the ASCII protocol), mirroring the reference client. + +## Test / verify + +`run.sh` runs the CRC self-test plus an **end-to-end** interop test: it builds +the C++ device shim (`../interop/odrive_native_interop_device.cpp`), spawns it on +a PTY, connects **this** client, enumerates the tree, and read/write-verifies +`vbus_voltage`, `serial_number`, `axis0.controller.input_pos`, and +`axis0.controller.config.vel_limit`. + +```bash +./run.sh # uses ./.venv if present, else creates one with pyserial +``` + +Expected tail: + +``` +[test] ALL END-TO-END ASSERTIONS PASSED (espp_odrive client <-> espp device) +==================== SUMMARY ==================== +RESULT: PASS +``` diff --git a/components/odrive_native/python/espp_odrive/__init__.py b/components/odrive_native/python/espp_odrive/__init__.py new file mode 100644 index 000000000..1500734be --- /dev/null +++ b/components/odrive_native/python/espp_odrive/__init__.py @@ -0,0 +1,65 @@ +"""espp_odrive -- a clean, dependency-light Python client for the ODrive +legacy *native* (Fibre endpoint) protocol. + +This is an odrivetool-equivalent re-implementation of the documented wire +protocol (see ``PROTOCOL.md``). It depends only on the Python standard library +plus ``pyserial`` for the serial backend -- there is **no** dependency on the +``odrive``/``fibre`` pip package. + +Quick start:: + + from espp_odrive import connect + dev = connect("/dev/ttyUSB0") + print("vbus:", dev.vbus_voltage) + dev.axis0.controller.input_pos = 3.14 + dev.dump() +""" + +from .ascii import OdriveAscii +from .crc import PROTOCOL_VERSION, crc8, crc16 +from .device import ( + CanaryMismatch, + Channel, + Device, + OdriveError, + RemoteObject, + RemoteProperty, + TimeoutError_, + connect, + connect_transport, + find, +) +from .protocol import TYPE_CODECS, build_packet, parse_response +from .transport import ( + SerialStreamTransport, + StreamDeframer, + Transport, + stream_frame, +) + +__version__ = "0.1.0" + +__all__ = [ + "connect", + "connect_transport", + "find", + "Device", + "RemoteObject", + "RemoteProperty", + "Channel", + "OdriveError", + "TimeoutError_", + "CanaryMismatch", + "Transport", + "SerialStreamTransport", + "StreamDeframer", + "stream_frame", + "TYPE_CODECS", + "build_packet", + "parse_response", + "crc8", + "crc16", + "PROTOCOL_VERSION", + "OdriveAscii", + "__version__", +] diff --git a/components/odrive_native/python/espp_odrive/ascii.py b/components/odrive_native/python/espp_odrive/ascii.py new file mode 100644 index 000000000..7b982e02f --- /dev/null +++ b/components/odrive_native/python/espp_odrive/ascii.py @@ -0,0 +1,67 @@ +"""Thin helper for the ODrive *ASCII* protocol (separate from the native one). + +This is deliberately minimal: it just formats and sends the documented +``r/w/p/v/f`` text lines over a serial port and reads back a line for the +commands that reply. It shares nothing with the binary native protocol; use it +only if you specifically want the ASCII interface. + + a = OdriveAscii("/dev/ttyUSB0") + a.position(0, 3.14) # p 0 3.14 0 0 + a.velocity(0, 5.0) # v 0 5.0 0 + pos, vel = a.feedback(0) # f 0 -> " " + vbus = float(a.read("vbus_voltage")) + a.write("axis0.controller.input_pos", 1.0) +""" + + +class OdriveAscii: + def __init__(self, port: str, baudrate: int = 115200, serial_obj=None): + if serial_obj is not None: + self._serial = serial_obj + else: + import serial + self._serial = serial.Serial(port, baudrate, timeout=1.0) + + def _send(self, line: str) -> None: + self._serial.write((line + "\n").encode("ascii")) + self._serial.flush() + + def _send_recv(self, line: str) -> str: + self._send(line) + return self._serial.readline().decode("ascii").strip() + + def read(self, name: str) -> str: + """``r `` -- returns the raw string the device replies with.""" + return self._send_recv("r " + name) + + def write(self, name: str, value) -> None: + """``w ``.""" + self._send("w %s %s" % (name, value)) + + def position(self, motor: int, pos, vel_ff=0, torque_ff=0) -> None: + """``p ``.""" + self._send("p %d %s %s %s" % (motor, pos, vel_ff, torque_ff)) + + def velocity(self, motor: int, vel, torque_ff=0) -> None: + """``v ``.""" + self._send("v %d %s %s" % (motor, vel, torque_ff)) + + def feedback(self, motor: int): + """``f `` -- returns ``(pos, vel)`` as floats.""" + reply = self._send_recv("f %d" % motor) + parts = reply.split() + return (float(parts[0]), float(parts[1])) if len(parts) >= 2 else (None, None) + + def close(self) -> None: + try: + self._serial.close() + except Exception: + # Best-effort close: the port may already be gone (device + # unplugged) or never fully opened; nothing useful to do on error. + pass + + def __enter__(self): + return self + + def __exit__(self, *exc): + self.close() diff --git a/components/odrive_native/python/espp_odrive/crc.py b/components/odrive_native/python/espp_odrive/crc.py new file mode 100644 index 000000000..17e4c180d --- /dev/null +++ b/components/odrive_native/python/espp_odrive/crc.py @@ -0,0 +1,52 @@ +"""ODrive legacy (Fibre) CRC primitives. + +Both widths use the same bit-by-bit, **non-reflected, MSB-first** algorithm. +This is a clean re-implementation of the documented wire algorithm (see +``PROTOCOL.md``); it does NOT depend on the ``odrive``/``fibre`` pip package. + +Constants (from the fw-v0.5.1 reference): + +* CRC8 -- poly ``0x37``, init ``0x42`` (UART *stream* framing header only) +* CRC16 -- poly ``0x3d65``, init ``0x1337`` (stream framing + endpoint-0 trailer) +* the endpoint ``json_crc`` canary is CRC16 seeded with ``PROTOCOL_VERSION`` (1), + **not** ``0x1337``. +""" + +CRC8_POLY = 0x37 +CRC8_INIT = 0x42 +CRC16_POLY = 0x3D65 +CRC16_INIT = 0x1337 +PROTOCOL_VERSION = 1 + + +def calc_crc(remainder: int, value: int, poly: int, bitwidth: int) -> int: + """Fold a single byte ``value`` through the running ``remainder``.""" + topbit = 1 << (bitwidth - 1) + mask = (1 << bitwidth) - 1 + remainder ^= (value << (bitwidth - 8)) & mask + for _ in range(8): + if remainder & topbit: + remainder = ((remainder << 1) ^ poly) & mask + else: + remainder = (remainder << 1) & mask + return remainder & mask + + +def crc8(data: bytes, init: int = CRC8_INIT) -> int: + """CRC8 over ``data`` (poly 0x37).""" + rem = init + for b in data: + rem = calc_crc(rem, b, CRC8_POLY, 8) + return rem + + +def crc16(data: bytes, init: int = CRC16_INIT) -> int: + """CRC16 over ``data`` (poly 0x3d65). + + Use ``init=CRC16_INIT`` (0x1337) for stream framing, or + ``init=PROTOCOL_VERSION`` (1) for the endpoint ``json_crc`` canary. + """ + rem = init + for b in data: + rem = calc_crc(rem, b, CRC16_POLY, 16) + return rem diff --git a/components/odrive_native/python/espp_odrive/device.py b/components/odrive_native/python/espp_odrive/device.py new file mode 100644 index 000000000..b14ce8cd6 --- /dev/null +++ b/components/odrive_native/python/espp_odrive/device.py @@ -0,0 +1,354 @@ +"""High-level ODrive native client: channel, object tree, connect/find. + +Usage:: + + from espp_odrive import connect + dev = connect("/dev/ttyUSB0") + print(dev.vbus_voltage) # typed read + dev.axis0.controller.input_pos = 3.14 # typed write + dev.dump() # pretty-print the live tree +""" + +import json +import struct +import time + +from .crc import PROTOCOL_VERSION, crc16 +from .protocol import TYPE_CODECS, build_packet, parse_response +from .transport import SerialStreamTransport, Transport + + +class OdriveError(Exception): + """Base class for all client errors.""" + + +class TimeoutError_(OdriveError): + """No response arrived before the deadline.""" + + +class CanaryMismatch(OdriveError): + """The device rejected our packet (json_crc / protocol-version mismatch).""" + + +# --------------------------------------------------------------------------- # +# Channel: sequence numbers + synchronous request/response over a Transport +# --------------------------------------------------------------------------- # +class Channel: + """Owns the outbound sequence counter and the endpoint request/response loop. + + ``json_crc`` is the interface-definition canary; it is 0 until the JSON tree + has been downloaded, which is fine because endpoint-0 reads use + ``PROTOCOL_VERSION`` as their trailer. + """ + + def __init__(self, transport: Transport, timeout: float = 2.0): + self._transport = transport + self._timeout = timeout + self._seq = 0 + self.json_crc = 0 + + def endpoint_operation(self, endpoint_id: int, payload: bytes = b"", + expect_response: bool = True, output_len: int = 0) -> bytes: + """Perform one endpoint read/write and return the response data bytes. + + Writes carry a non-empty ``payload``; reads set ``output_len`` to the + number of bytes wanted back. A packet can do both at once. + """ + if len(payload) >= 128: + raise OdriveError("payload larger than 127 bytes is not supported") + self._seq = (self._seq + 1) & 0x7FFF + # One bit is hardwired to 1 to avoid clashing with the ASCII protocol, + # mirroring the reference fibre client. + seq = self._seq | 0x80 + packet = build_packet(seq, endpoint_id, output_len, payload, + expect_response, self.json_crc) + self._transport.send_packet(packet) + if not expect_response: + return b"" + + deadline = time.monotonic() + self._timeout + while True: + remaining = deadline - time.monotonic() + if remaining <= 0: + raise TimeoutError_( + "no response for endpoint %d (seq %d) within %.1fs" + % (endpoint_id & 0x7FFF, seq, self._timeout)) + resp = self._transport.read_packet(remaining) + if resp is None: + continue + parsed = parse_response(resp) + if parsed is None: + continue + seq_no, data = parsed + if (seq_no & 0x8000) and (seq_no & 0x7FFF) == (seq & 0x7FFF): + return data + # else: stale/unmatched response, keep waiting + + def read_endpoint_buffer(self, endpoint_id: int) -> bytes: + """Read a long endpoint (e.g. endpoint 0 JSON) in chunks by offset.""" + buffer = b"" + while True: + chunk = self.endpoint_operation( + endpoint_id, struct.pack("" % e + if self._name == "serial_number": + return "0x%012X" % v + if self._name == "error" or self._name.endswith("_error"): + return "0x%X" % v + return repr(v) + + +class RemoteObject: + """A branch node exposing children (sub-objects, properties) as attributes.""" + + def __init__(self, channel: Channel, name: str = ""): + # Everything private goes through object.__setattr__ to avoid the + # attribute-forwarding in __setattr__ below. + object.__setattr__(self, "_channel", channel) + object.__setattr__(self, "_name", name) + object.__setattr__(self, "_children", {}) # name -> RemoteObject | RemoteProperty + object.__setattr__(self, "_sealed", False) + + # -- tree construction -------------------------------------------------- + def _add(self, name, child): + self._children[name] = child + + def _seal(self): + object.__setattr__(self, "_sealed", True) + + # -- attribute access --------------------------------------------------- + def __getattr__(self, name): + # Only called when normal lookup fails (i.e. not a real attribute). + children = object.__getattribute__(self, "_children") + if name in children: + child = children[name] + if isinstance(child, RemoteProperty): + if not child.can_read: + # __getattr__ must raise AttributeError (never a custom + # exception): a write-only property has no readable value, + # and hasattr()/getattr() default handling relies on this. + raise AttributeError("property %s is write-only" % name) + return child.get_value() + return child + raise AttributeError(name) + + def __setattr__(self, name, value): + children = object.__getattribute__(self, "_children") + child = children.get(name) + if isinstance(child, RemoteProperty): + child.set_value(value) + return + if isinstance(child, RemoteObject): + raise OdriveError("cannot assign to sub-object %s" % name) + object.__setattr__(self, name, value) + + def __dir__(self): + return sorted(set(list(super().__dir__()) + list(self._children.keys()))) + + # -- introspection ------------------------------------------------------ + def get_property(self, name) -> RemoteProperty: + """Return the underlying :class:`RemoteProperty` (no read triggered).""" + child = self._children.get(name) + if not isinstance(child, RemoteProperty): + raise KeyError(name) + return child + + def _dump_lines(self, indent, out): + for key, child in self._children.items(): + if isinstance(child, RemoteObject): + out.append("%s%s:" % (indent, key)) + child._dump_lines(indent + " ", out) + else: + out.append("%s%s = %s (%s)" % (indent, key, child._format_value(), child._type)) + + +class Device(RemoteObject): + """The root object returned by :func:`connect` / :func:`find`. + + In addition to the attribute tree it holds the transport, the raw JSON + descriptor, and the computed ``json_crc``. + """ + + def __init__(self, channel: Channel, transport: Transport, + json_bytes: bytes, json_data): + super().__init__(channel, name="") + object.__setattr__(self, "_transport", transport) + object.__setattr__(self, "_json_bytes", json_bytes) + object.__setattr__(self, "_json_data", json_data) + object.__setattr__(self, "json_crc", channel.json_crc) + + # -- convenience path get/set ------------------------------------------ + def get(self, path: str): + """Typed read of a dotted path, e.g. ``dev.get("axis0.error")``.""" + return self._resolve(path).get_value() + + def set(self, path: str, value): + """Typed write of a dotted path, e.g. ``dev.set("axis0.controller.input_pos", 1.0)``.""" + self._resolve(path).set_value(value) + + def _resolve(self, path: str) -> RemoteProperty: + obj = self + parts = path.split(".") + for p in parts[:-1]: + obj = object.__getattribute__(obj, "_children")[p] + prop = object.__getattribute__(obj, "_children")[parts[-1]] + if not isinstance(prop, RemoteProperty): + raise KeyError("%s is not a property" % path) + return prop + + def dump(self) -> str: + """Pretty-print the whole tree with live values; returns the string too.""" + out = [] + self._dump_lines("", out) + text = "\n".join(out) + print(text) + return text + + def close(self): + self._transport.close() + + def __enter__(self): + return self + + def __exit__(self, *exc): + self.close() + + +# --------------------------------------------------------------------------- # +# Tree building from the endpoint-0 JSON descriptor +# --------------------------------------------------------------------------- # +def _build_tree(channel: Channel, parent: RemoteObject, members): + for m in members: + name = m.get("name") + if name is None: + continue + type_str = m.get("type") + if type_str == "object": + child = RemoteObject(channel, name=name) + _build_tree(channel, child, m.get("members", [])) + child._seal() + parent._add(name, child) + elif type_str == "function": + # Functions are out of scope for this client; skip cleanly. + continue + elif type_str is not None: + ep_id = m.get("id") + if ep_id is None: + continue + parent._add(name, RemoteProperty( + channel, name, int(ep_id), type_str, m.get("access", "r"))) + + +# --------------------------------------------------------------------------- # +# Entry points +# --------------------------------------------------------------------------- # +def _device_from_transport(transport: Transport, timeout: float = 2.0) -> Device: + channel = Channel(transport, timeout=timeout) + # Endpoint 0 (JSON descriptor) reads use PROTOCOL_VERSION as the trailer, so + # this works before json_crc is known. + json_bytes = channel.read_endpoint_buffer(0) + if not json_bytes: + raise OdriveError("device returned an empty endpoint-0 JSON descriptor") + try: + json_str = json_bytes.decode("ascii") + except UnicodeDecodeError as e: + raise OdriveError("endpoint-0 descriptor is not ASCII: %r" % e) + json_data = json.loads(json_str) + # The endpoint canary is CRC16 over the exact JSON bytes, seeded with + # PROTOCOL_VERSION (NOT the 0x1337 stream init). + channel.json_crc = crc16(json_bytes, PROTOCOL_VERSION) + + device = Device(channel, transport, json_bytes, json_data) + _build_tree(channel, device, json_data) + device._seal() + return device + + +def connect(port: str, baudrate: int = 115200, timeout: float = 2.0) -> Device: + """Connect to an ODrive over a serial/UART port and return a :class:`Device`. + + Downloads the endpoint-0 JSON descriptor, computes ``json_crc``, and builds + the object tree. + """ + transport = SerialStreamTransport(port, baudrate=baudrate) + try: + return _device_from_transport(transport, timeout=timeout) + except Exception: + transport.close() + raise + + +def connect_transport(transport: Transport, timeout: float = 2.0) -> Device: + """Like :func:`connect`, but over an already-constructed :class:`Transport`. + + This is the seam for a future USB-bulk backend: build the transport, then + hand it here. + """ + return _device_from_transport(transport, timeout=timeout) + + +def find(timeout: float = 5.0, baudrate: int = 115200): + """Scan available serial ports and return the first ODrive that answers. + + Returns a :class:`Device` or ``None`` if none was found within ``timeout``. + """ + from serial.tools import list_ports + + deadline = time.monotonic() + timeout + tried = set() + while time.monotonic() < deadline: + for info in list_ports.comports(): + if info.device in tried: + continue + tried.add(info.device) + try: + return connect(info.device, baudrate=baudrate, + timeout=min(2.0, max(0.5, deadline - time.monotonic()))) + except Exception: + continue + time.sleep(0.2) + return None diff --git a/components/odrive_native/python/espp_odrive/protocol.py b/components/odrive_native/python/espp_odrive/protocol.py new file mode 100644 index 000000000..d87bc1e7b --- /dev/null +++ b/components/odrive_native/python/espp_odrive/protocol.py @@ -0,0 +1,85 @@ +"""ODrive legacy (Fibre) endpoint protocol -- packet codec + type codecs. + +A clean re-implementation of the documented wire format (``PROTOCOL.md``), +with no dependency on the ``odrive``/``fibre`` pip package. + +Packet (little-endian throughout):: + + request : [seq u16][endpoint u16 (bit15 => expect response)][output_len u16][payload][trailer u16] + response: [seq | 0x8000 u16][data ...] + +The ``trailer`` is a canary the server checks: ``PROTOCOL_VERSION`` for +endpoint 0, else the ``json_crc``. +""" + +import struct + +from .crc import PROTOCOL_VERSION + +__all__ = [ + "PROTOCOL_VERSION", + "TYPE_CODECS", + "TypeCodec", + "build_packet", + "parse_response", +] + + +class TypeCodec: + """(de)serializer for one primitive wire type, backed by ``struct``.""" + + __slots__ = ("name", "fmt", "size", "py_type") + + def __init__(self, name: str, fmt: str, py_type): + self.name = name + self.fmt = "<" + fmt + self.size = struct.calcsize(self.fmt) + self.py_type = py_type + + def encode(self, value) -> bytes: + return struct.pack(self.fmt, self.py_type(value)) + + def decode(self, data: bytes): + # Accept a short/long buffer defensively; only the leading bytes matter. + return struct.unpack(self.fmt, data[: self.size])[0] + + +# All little-endian; sizes 1/1/1/2/2/4/4/8/8/4 per PROTOCOL.md. +TYPE_CODECS = { + "bool": TypeCodec("bool", "?", bool), + "int8": TypeCodec("int8", "b", int), + "uint8": TypeCodec("uint8", "B", int), + "int16": TypeCodec("int16", "h", int), + "uint16": TypeCodec("uint16", "H", int), + "int32": TypeCodec("int32", "i", int), + "uint32": TypeCodec("uint32", "I", int), + "int64": TypeCodec("int64", "q", int), + "uint64": TypeCodec("uint64", "Q", int), + "float": TypeCodec("float", "f", float), +} + + +def build_packet(seq: int, endpoint_id: int, output_len: int, payload: bytes, + expect_response: bool, json_crc: int) -> bytes: + """Assemble one request packet (without stream framing). + + ``seq`` should be the 15-bit outbound sequence number; the caller keeps it + unique. ``endpoint_id`` is the low 15-bit endpoint number. + """ + ep_field = (endpoint_id & 0x7FFF) | (0x8000 if expect_response else 0) + packet = struct.pack(" bytes: + """Wrap one packet in a fibre serial stream frame. + + ``[0xAA][len u8][crc8(sync,len) init 0x42][packet][crc16(packet) init 0x1337, big-endian]`` + """ + if len(packet) >= MAX_PACKET_SIZE: + raise ValueError("packet larger than 127 bytes is not supported by stream framing") + header = bytes([SYNC_BYTE, len(packet)]) + header += bytes([crc8(header)]) + trailer = struct.pack(">H", crc16(packet, CRC16_INIT)) # big-endian + return header + packet + trailer + + +class StreamDeframer: + """Stateful deframer: feed received bytes, get back complete packets. + + Resynchronizes on the ``0xAA`` sync byte and validates both CRCs; a frame + that fails either CRC (or carries ``len >= 128``) is dropped and the + deframer hunts for the next sync byte. + """ + + def __init__(self): + self._buf = bytearray() + + def push(self, data: bytes): + self._buf.extend(data) + packets = [] + while True: + # Resync to the first sync byte. + sync = self._buf.find(SYNC_BYTE) + if sync < 0: + self._buf.clear() + break + if sync > 0: + del self._buf[:sync] + if len(self._buf) < 3: + break + length = self._buf[1] + if length >= MAX_PACKET_SIZE or crc8(bytes(self._buf[:3])) != 0: + # Bad header: drop the sync byte and hunt for the next one. + del self._buf[0] + continue + frame_len = 3 + length + 2 + if len(self._buf) < frame_len: + break # wait for more bytes + packet = bytes(self._buf[3:3 + length]) + got = struct.unpack(">H", bytes(self._buf[3 + length:3 + length + 2]))[0] + if crc16(packet, CRC16_INIT) != got: + del self._buf[0] + continue + packets.append(packet) + del self._buf[:frame_len] + return packets + + +# --------------------------------------------------------------------------- # +# Transport interface + serial backend +# --------------------------------------------------------------------------- # +class Transport(ABC): + """Moves whole packets to/from the device.""" + + @abstractmethod + def send_packet(self, packet: bytes) -> None: + raise NotImplementedError + + @abstractmethod + def read_packet(self, timeout: float): + """Return one received packet, or ``None`` if none arrived in ``timeout``.""" + raise NotImplementedError + + def close(self) -> None: + # Default no-op; subclasses with a real resource override this. + return + + def __enter__(self): + return self + + def __exit__(self, *exc): + self.close() + + +class SerialStreamTransport(Transport): + """Serial/UART backend: stream-frames outgoing packets and deframes input.""" + + def __init__(self, port: str, baudrate: int = 115200, serial_obj=None): + if serial_obj is not None: + self._serial = serial_obj + else: + import serial # pyserial; imported lazily so USB-only users need not install it + self._serial = serial.Serial(port, baudrate, timeout=0) + self._deframer = StreamDeframer() + self._pending = [] + + def send_packet(self, packet: bytes) -> None: + self._serial.write(stream_frame(packet)) + self._serial.flush() + + def read_packet(self, timeout: float): + if self._pending: + return self._pending.pop(0) + deadline = time.monotonic() + max(0.0, timeout) + while True: + data = self._serial.read(256) + if data: + new = self._deframer.push(data) + if new: + self._pending.extend(new) + return self._pending.pop(0) + if time.monotonic() >= deadline: + return None + if not data: + time.sleep(0.001) + + def close(self) -> None: + try: + self._serial.close() + except Exception: + # Best-effort close: the port may already be gone (device + # unplugged) or never fully opened; nothing useful to do on error. + pass diff --git a/components/odrive_native/python/run.sh b/components/odrive_native/python/run.sh new file mode 100755 index 000000000..5d9f59ded --- /dev/null +++ b/components/odrive_native/python/run.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash +# Run the espp_odrive client test suite (CRC self-test + end-to-end interop +# against the C++ device shim over a PTY). +# +# Uses ./.venv if present (created with pyserial); otherwise falls back to +# $PYTHON / python3 (which must have pyserial installed). +set -uo pipefail +cd "$(dirname "$0")" + +if [ -x ".venv/bin/python" ]; then + PY=".venv/bin/python" +else + PY="${PYTHON:-python3}" + if ! "$PY" -c "import serial" 2>/dev/null; then + echo "creating .venv with pyserial..." + "$PY" -m venv .venv \ + && .venv/bin/python -m pip install --quiet --upgrade pip pyserial \ + && PY=".venv/bin/python" + fi +fi + +echo "using python: $PY ($($PY --version 2>&1))" +exec "$PY" tests/test_odrive.py diff --git a/components/odrive_native/python/tests/test_odrive.py b/components/odrive_native/python/tests/test_odrive.py new file mode 100755 index 000000000..1c762beff --- /dev/null +++ b/components/odrive_native/python/tests/test_odrive.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +"""End-to-end + CRC self-test for the espp_odrive native client. + +Runs two things: + +1. **CRC self-test** -- the golden vector ``crc16("123456789", init=0x1337) == + 0xaa01`` plus a stream-frame round-trip through the deframer. +2. **End-to-end interop** -- builds the C++ device shim, spawns it on a PTY, + connects THIS client (not the reference fibre package), enumerates the tree, + reads ``vbus_voltage``/``serial_number``, and write-then-reads + ``axis0.controller.input_pos`` and ``axis0.controller.config.vel_limit``. + +Runnable directly (``python3 tests/test_odrive.py``); also exposes ``test_*`` +functions for pytest if it happens to be available. +""" +import os +import re +import subprocess +import sys +import time + +HERE = os.path.dirname(os.path.abspath(__file__)) +PKG_ROOT = os.path.dirname(HERE) # .../python +COMPONENT = os.path.dirname(PKG_ROOT) # .../odrive_native +INCLUDE = os.path.join(COMPONENT, "include") +DEVICE_SRC = os.path.join(COMPONENT, "interop", "odrive_native_interop_device.cpp") + +sys.path.insert(0, PKG_ROOT) + +from espp_odrive import connect # noqa: E402 +from espp_odrive.crc import crc16, crc8, CRC16_INIT, CRC8_INIT # noqa: E402 +from espp_odrive.transport import StreamDeframer, stream_frame # noqa: E402 + + +def log(msg): + print("[test] " + msg, flush=True) + + +# --------------------------------------------------------------------------- # +# 1. CRC self-test +# --------------------------------------------------------------------------- # +def test_crc_golden(): + got = crc16(b"123456789", CRC16_INIT) + assert got == 0xAA01, "crc16('123456789', 0x1337) = 0x%04x, expected 0xaa01" % got + # CRC8 init/self-consistency: crc8 over [sync,len,crc8] is 0. + hdr = bytes([0xAA, 5]) + hdr += bytes([crc8(hdr, CRC8_INIT)]) + assert crc8(hdr, CRC8_INIT) == 0, "crc8 header self-check failed" + log("CRC self-test PASSED (crc16('123456789')=0x%04x)" % got) + + +def test_stream_roundtrip(): + packet = bytes(range(20)) + framed = stream_frame(packet) + deframer = StreamDeframer() + # Feed it in two arbitrary splits + some leading garbage to test resync. + out = deframer.push(b"\x00\x01" + framed[:3]) + out += deframer.push(framed[3:]) + assert out == [packet], "stream round-trip failed: %r" % out + log("stream frame/deframe round-trip PASSED") + + +# --------------------------------------------------------------------------- # +# 2. End-to-end interop against the device shim +# --------------------------------------------------------------------------- # +def _build_device(workdir): + out = os.path.join(workdir, "odrive_native_device") + cxx = os.environ.get("CXX", "c++") + cmd = [cxx, "-std=c++20", "-I", INCLUDE, DEVICE_SRC, "-o", out] + log("building device shim: " + " ".join(cmd)) + subprocess.run(cmd, check=True) + return out + + +def _spawn_device(device_bin): + proc = subprocess.Popen([device_bin], stdout=subprocess.PIPE, + stderr=subprocess.PIPE, bufsize=1, text=True) + pty = None + deadline = time.monotonic() + 10 + while time.monotonic() < deadline: + line = proc.stdout.readline() + if not line: + if proc.poll() is not None: + break + continue + m = re.match(r"PTY_SLAVE (\S+)", line.strip()) + if m: + pty = m.group(1) + break + return proc, pty + + +def test_end_to_end(): + workdir = os.environ.get("TMPDIR", "/tmp") + device_bin = _build_device(workdir) + proc, pty = _spawn_device(device_bin) + try: + assert pty, "device shim did not report a PTY slave" + log("device PTY slave = %s" % pty) + + dev = connect(pty, timeout=5.0) + log("CONNECTED. json_crc = 0x%04x, descriptor = %d bytes" + % (dev.json_crc, len(dev._json_bytes))) + + # Enumerate the tree (dump() logs it; the return value is not needed). + log("endpoint tree:") + dev.dump() + + # Read values. + vbus = dev.vbus_voltage + log("READ vbus_voltage = %r" % vbus) + assert abs(vbus - 24.37) < 1e-3, "vbus mismatch: %r" % vbus + + sn = dev.serial_number + log("READ serial_number = 0x%X" % sn) + assert sn == 0x00A1B2C3D4E5, "serial_number mismatch: 0x%X" % sn + + err = dev.axis0.error + log("READ axis0.error = %r" % err) + assert err == 0 + + # Write then read back (attribute style). + dev.axis0.controller.input_pos = 3.14159 + rb = dev.axis0.controller.input_pos + log("WRITE/READ axis0.controller.input_pos = %r" % rb) + assert abs(rb - 3.14159) < 1e-4, "input_pos read-back mismatch: %r" % rb + + # Write then read back (get/set path style). + dev.set("axis0.controller.config.vel_limit", 42.5) + vlim = dev.get("axis0.controller.config.vel_limit") + log("WRITE/READ axis0.controller.config.vel_limit = %r" % vlim) + assert abs(vlim - 42.5) < 1e-4, "vel_limit read-back mismatch: %r" % vlim + + log("ALL END-TO-END ASSERTIONS PASSED (espp_odrive client <-> espp device)") + dev.close() + finally: + proc.terminate() + try: + proc.wait(timeout=3) + except Exception: + proc.kill() + + +def main(): + rc = 0 + for fn in (test_crc_golden, test_stream_roundtrip, test_end_to_end): + try: + fn() + except Exception as e: + import traceback + traceback.print_exc() + log("FAILED: %s: %s" % (fn.__name__, e)) + rc = 1 + print("\n==================== SUMMARY ====================") + print("RESULT: %s" % ("PASS" if rc == 0 else "FAIL")) + return rc + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/components/odrive_native/test/odrive_native_host_test.cpp b/components/odrive_native/test/odrive_native_host_test.cpp new file mode 100644 index 000000000..a8b564910 --- /dev/null +++ b/components/odrive_native/test/odrive_native_host_test.cpp @@ -0,0 +1,214 @@ +// Host-buildable unit tests for the ODrive legacy native (Fibre endpoint) +// protocol wire core. Build & run with: +// c++ -std=c++20 -I../include odrive_native_host_test.cpp -o test && ./test +// +// These tests exercise espp::detail::OdriveNativeCore directly so they need no +// ESP-IDF headers. + +#include +#include +#include +#include +#include +#include +#include + +#include "detail/odrive_native_core.hpp" + +using espp::detail::odrive_crc16; +using espp::detail::OdriveNativeCore; + +static int g_failures = 0; +#define CHECK(cond) \ + do { \ + if (!(cond)) { \ + std::printf(" FAIL: %s (line %d)\n", #cond, __LINE__); \ + ++g_failures; \ + } \ + } while (0) + +// --- packet building helpers --------------------------------------------- +static void put_u16(std::vector &v, uint16_t x) { + v.push_back(uint8_t(x & 0xff)); + v.push_back(uint8_t((x >> 8) & 0xff)); +} +static void put_u32(std::vector &v, uint32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(uint8_t((x >> (8 * i)) & 0xff)); +} + +// Build a request packet: [seq][endpoint_field][output_len][payload][trailer] +static std::vector make_packet(uint16_t seq, uint16_t endpoint_id, bool expect_response, + uint16_t output_len, std::span payload, + uint16_t trailer) { + std::vector p; + put_u16(p, seq); + put_u16(p, uint16_t(endpoint_id | (expect_response ? 0x8000 : 0))); + put_u16(p, output_len); + p.insert(p.end(), payload.begin(), payload.end()); + put_u16(p, trailer); + return p; +} + +static void test_crc_golden() { + std::printf("test_crc_golden\n"); + CHECK(odrive_crc16(std::string_view("")) == 0x1337); + const uint8_t zero = 0x00; + CHECK(odrive_crc16(std::span(&zero, 1)) == 0xe150); + CHECK(odrive_crc16(std::string_view("123456789")) == 0xaa01); + std::vector v0_19; + for (int i = 0; i < 20; ++i) + v0_19.push_back(uint8_t(i)); + CHECK(odrive_crc16(v0_19) == 0x94d3); + CHECK(odrive_crc16(std::string_view( + "[{\"name\":\"vbus_voltage\",\"id\":1,\"type\":\"float\",\"access\":\"r\"}]")) == + 0x59ec); +} + +static void test_endpoint0_read() { + std::printf("test_endpoint0_read\n"); + OdriveNativeCore core; + float vbus = 24.0f; + core.register_float_property("vbus_voltage", [&]() { return vbus; }); + core.register_float_property( + "axis0.controller.input_pos", [&]() { return 0.0f; }, + [&](float, std::error_code &) { return true; }); + + const std::string json = core.json(); + // The endpoint canary (interface-definition CRC) is CRC-16 over the exact JSON + // bytes seeded with PROTOCOL_VERSION (1) -- matching the fw-v0.5.1 firmware and + // the reference fibre client (verified by the interop harness), NOT the 0x1337 + // packet-CRC init. + CHECK(core.json_crc() == odrive_crc16(json, espp::detail::kProtocolVersion)); + + // Read endpoint 0 from offset 0, want up to 512 bytes. + std::vector off0; + put_u32(off0, 0); + auto req = make_packet(0x0005, /*endpoint*/ 0, /*expect*/ true, /*output_len*/ 512, off0, + /*trailer*/ 1 /*PROTOCOL_VERSION*/); + auto resp = core.process_bytes(req); + CHECK(resp.size() >= 2); + uint16_t resp_seq = uint16_t(resp[0] | (resp[1] << 8)); + CHECK((resp_seq & 0x8000) != 0); + CHECK((resp_seq & 0x7fff) == 0x0005); + std::string data(resp.begin() + 2, resp.end()); + CHECK(data == json); + + // Second read at the returned length -> empty data (terminates read loop). + std::vector off_end; + put_u32(off_end, uint32_t(json.size())); + auto req2 = make_packet(0x0006, 0, true, 512, off_end, 1); + auto resp2 = core.process_bytes(req2); + CHECK(resp2.size() == 2); // just the seq header, no data + uint16_t resp2_seq = uint16_t(resp2[0] | (resp2[1] << 8)); + CHECK((resp2_seq & 0x8000) != 0); +} + +static void test_float_write_then_read() { + std::printf("test_float_write_then_read\n"); + OdriveNativeCore core; + float stored = 0.0f; + bool getter_called = false, setter_called = false; + // vbus_voltage is endpoint id 1, input_pos is endpoint id 2 (rw). + core.register_float_property("vbus_voltage", [&]() { return 24.0f; }); + core.register_float_property( + "axis0.controller.input_pos", + [&]() { + getter_called = true; + return stored; + }, + [&](float v, std::error_code &ec) { + setter_called = true; + stored = v; + ec.clear(); + return true; + }); + const uint16_t crc = core.json_crc(); + const uint16_t ep = 2; + + // Write 12.5f to endpoint 2. + const float wrote = 12.5f; + std::vector payload(4); + std::memcpy(payload.data(), &wrote, 4); + auto wreq = make_packet(0x0010, ep, /*expect*/ true, /*output_len*/ 0, payload, crc); + auto wresp = core.process_bytes(wreq); + CHECK(setter_called); + CHECK(stored == wrote); + CHECK(wresp.size() == 2); // header only, no data for a pure write + + // Read it back (output_len=4, empty payload). + auto rreq = make_packet(0x0011, ep, true, 4, std::span{}, crc); + auto rresp = core.process_bytes(rreq); + CHECK(getter_called); + CHECK(rresp.size() == 2 + 4); + float readback = 0.0f; + std::memcpy(&readback, rresp.data() + 2, 4); + CHECK(readback == wrote); +} + +static void test_canary_rejection() { + std::printf("test_canary_rejection\n"); + OdriveNativeCore core; + float stored = 1.0f; + bool setter_called = false; + core.register_float_property( + "axis0.controller.input_pos", [&]() { return stored; }, + [&](float v, std::error_code &ec) { + setter_called = true; + stored = v; + ec.clear(); + return true; + }); + const uint16_t good_crc = core.json_crc(); + const uint16_t bad_crc = uint16_t(good_crc ^ 0xffff); + const uint16_t ep = 1; + + const float wrote = 99.0f; + std::vector payload(4); + std::memcpy(payload.data(), &wrote, 4); + auto wreq = make_packet(0x0020, ep, true, 0, payload, bad_crc); + auto wresp = core.process_bytes(wreq); + CHECK(wresp.empty()); // ignored + CHECK(!setter_called); // no callback + CHECK(stored == 1.0f); // no state change +} + +static void test_no_response() { + std::printf("test_no_response\n"); + OdriveNativeCore core; + core.register_float_property("vbus_voltage", [&]() { return 24.0f; }); + const uint16_t crc = core.json_crc(); + // Read endpoint 1 WITHOUT the expect-response bit -> empty output. + auto req = make_packet(0x0030, /*endpoint*/ 1, /*expect*/ false, /*output_len*/ 4, + std::span{}, crc); + auto resp = core.process_bytes(req); + CHECK(resp.empty()); +} + +static void test_unknown_endpoint_ignored() { + std::printf("test_unknown_endpoint_ignored\n"); + OdriveNativeCore core; + core.register_float_property("vbus_voltage", [&]() { return 24.0f; }); + const uint16_t crc = core.json_crc(); + // An unknown endpoint id, WITH the expect-response bit and a valid canary, + // must still be ignored (empty response) per PROTOCOL.md -- not ACKed. + auto req = make_packet(0x0031, /*endpoint*/ 999, /*expect*/ true, /*output_len*/ 4, + std::span{}, crc); + auto resp = core.process_bytes(req); + CHECK(resp.empty()); +} + +int main() { + test_crc_golden(); + test_endpoint0_read(); + test_float_write_then_read(); + test_canary_rejection(); + test_no_response(); + test_unknown_endpoint_ignored(); + if (g_failures == 0) { + std::printf("\nALL TESTS PASSED\n"); + return 0; + } + std::printf("\n%d CHECK(S) FAILED\n", g_failures); + return 1; +} diff --git a/components/odrive_native/test/odrive_native_stream_test.cpp b/components/odrive_native/test/odrive_native_stream_test.cpp new file mode 100644 index 000000000..b87aea5a3 --- /dev/null +++ b/components/odrive_native/test/odrive_native_stream_test.cpp @@ -0,0 +1,131 @@ +// Host-buildable golden tests for the ODrive legacy native (Fibre) UART *stream* +// framing. Build & run with: +// c++ -std=c++20 -I../include odrive_native_stream_test.cpp -o stream_test && ./stream_test +// +// These tests exercise espp::detail (stream_frame / StreamDeframer / odrive_crc8) +// directly, so they need no ESP-IDF headers. They freeze the wire framing that +// fibre's serial backend (Firmware/fibre/python/fibre/protocol.py) uses, verified +// against the fw-v0.5.1 reference. + +#include +#include +#include +#include +#include +#include + +#include "detail/odrive_native_stream.hpp" + +using espp::detail::odrive_crc8; +using espp::detail::stream_frame; +using espp::detail::StreamDeframer; + +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 std::string hex(std::span b) { + static const char *d = "0123456789ABCDEF"; + std::string s; + for (size_t i = 0; i < b.size(); ++i) { + if (i) + s += ' '; + s += d[b[i] >> 4]; + s += d[b[i] & 0xf]; + } + return s; +} + +static void test_crc8_golden() { + std::printf("test_crc8_golden\n"); + // crc8("") == init == 0x42 + CHECK(odrive_crc8(std::span{}) == 0x42); + const uint8_t zero = 0x00; + CHECK(odrive_crc8(std::span(&zero, 1)) == 0xca); + const char *s = "123456789"; + CHECK(odrive_crc8(std::span(reinterpret_cast(s), 9)) == 0x8c); + const uint8_t hdr[2] = {0xAA, 0x0A}; + CHECK(odrive_crc8(std::span(hdr, 2)) == 0x53); +} + +static void test_frame_golden() { + std::printf("test_frame_golden\n"); + // The endpoint-0 read request packet from the spec: + // seq=0x8080, endpoint=0x8000, output_len=512, offset u32=0, trailer=1 + const std::vector packet = {0x80, 0x80, 0x00, 0x80, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00}; + const std::vector expected = {0xAA, 0x0C, 0xE1, 0x80, 0x80, 0x00, 0x80, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0xA3, 0xAB}; + auto framed = stream_frame(packet); + CHECK(framed == expected); + if (framed != expected) { + std::printf(" got: %s\n", hex(framed).c_str()); + std::printf(" exp: %s\n", hex(expected).c_str()); + } + + // Receiver validation trick: crc8 over [sync,len,crc8] == 0. + const uint8_t hdr3[3] = {framed[0], framed[1], framed[2]}; + CHECK(odrive_crc8(std::span(hdr3, 3)) == 0); + // and crc16 over [packet .. crc16 bytes] == 0. + std::vector pk_plus(framed.begin() + 3, framed.end()); + CHECK(espp::detail::odrive_crc16(pk_plus) == 0); +} + +static void test_deframe_roundtrip() { + std::printf("test_deframe_roundtrip\n"); + const std::vector p1 = {0x80, 0x80, 0x00, 0x80, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00}; + const std::vector p2 = {0x81, 0x80, 0x01, 0x80, 0x04, 0x00, 0xEC, 0x59}; + auto f1 = stream_frame(p1); + auto f2 = stream_frame(p2); + + // Feed both frames concatenated in one push. + std::vector both = f1; + both.insert(both.end(), f2.begin(), f2.end()); + StreamDeframer d; + auto pkts = d.push(both); + CHECK(pkts.size() == 2); + if (pkts.size() == 2) { + CHECK(pkts[0] == p1); + CHECK(pkts[1] == p2); + } + CHECK(d.buffered() == 0); + + // Byte-at-a-time feed with leading garbage + a spurious 0xAA that fails header + // CRC -> the deframer must resync and still recover the packet. + StreamDeframer d2; + std::vector> got; + std::vector stream = {0x00, 0xFF, 0xAA, 0x7F, 0x13}; // junk incl. bad 0xAA header + stream.insert(stream.end(), f1.begin(), f1.end()); + for (uint8_t b : stream) { + auto r = d2.push(std::span(&b, 1)); + got.insert(got.end(), r.begin(), r.end()); + } + CHECK(got.size() == 1); + if (got.size() == 1) + CHECK(got[0] == p1); + + // A frame with a corrupted CRC16 trailer must be dropped (no packet yielded). + StreamDeframer d3; + auto bad = f1; + bad.back() ^= 0xFF; // corrupt low CRC16 byte + auto r3 = d3.push(bad); + CHECK(r3.empty()); +} + +int main() { + test_crc8_golden(); + test_frame_golden(); + test_deframe_roundtrip(); + if (g_failures == 0) { + std::printf("\nALL STREAM TESTS PASSED\n"); + return 0; + } + std::printf("\n%d CHECK(S) FAILED\n", g_failures); + return 1; +} diff --git a/doc/Doxyfile b/doc/Doxyfile index b04dbd0f4..86103c052 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -151,6 +151,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/neopixel/example/main/neopixel_example.cpp \ $(PROJECT_PATH)/components/nvs/example/main/nvs_example.cpp \ $(PROJECT_PATH)/components/odrive_ascii/example/main/odrive_ascii_example.cpp \ + $(PROJECT_PATH)/components/odrive_native/example/main/odrive_native_example.cpp \ $(PROJECT_PATH)/components/pca9535/example/main/pca9535_example.cpp \ $(PROJECT_PATH)/components/pcf85063/example/main/pcf85063_example.cpp \ $(PROJECT_PATH)/components/pid/example/main/pid_example.cpp \ @@ -362,6 +363,8 @@ INPUT = \ $(PROJECT_PATH)/components/meshtastic/include/meshtastic_types.hpp \ $(PROJECT_PATH)/components/mt6701/include/mt6701.hpp \ $(PROJECT_PATH)/components/odrive_ascii/include/odrive_ascii.hpp \ + $(PROJECT_PATH)/components/odrive_native/include/odrive_native.hpp \ + $(PROJECT_PATH)/components/odrive_native/include/detail/odrive_native_core.hpp \ $(PROJECT_PATH)/components/pca9535/include/pca9535.hpp \ $(PROJECT_PATH)/components/pcf85063/include/pcf85063.hpp \ $(PROJECT_PATH)/components/pid/include/pid.hpp \ diff --git a/doc/en/motor_control/index.rst b/doc/en/motor_control/index.rst index cc539999c..1b347e3bb 100644 --- a/doc/en/motor_control/index.rst +++ b/doc/en/motor_control/index.rst @@ -11,4 +11,5 @@ Motor-control algorithms and controller interfaces. See also the pid adrc odrive_ascii + odrive_native trajectory_planner diff --git a/doc/en/motor_control/odrive_native.rst b/doc/en/motor_control/odrive_native.rst new file mode 100644 index 000000000..0be34881e --- /dev/null +++ b/doc/en/motor_control/odrive_native.rst @@ -0,0 +1,74 @@ +ODrive Native (Fibre endpoint) Protocol Component +================================================= + +Overview +-------- + +``espp::OdriveNative`` implements a transport-agnostic server for the ODrive +legacy native (Fibre endpoint) binary protocol (firmware <= 0.5.x), as used over +the USB vendor interface where each bulk transfer carries exactly one packet. It +parses one inbound request packet and produces one response packet; it performs +no I/O itself. + +Applications register typed properties from dotted paths (mirroring +``espp::OdriveAscii``). Endpoint ids are assigned sequentially starting at 1 +(endpoint 0 is the JSON descriptor blob), and the compact JSON descriptor and its +CRC are finalized lazily. This lets a legacy ``odrivetool`` / ``fibre-python`` +client auto-discover the object tree and perform typed get/set. + +The CRC / packet packing / type codecs / JSON descriptor / dispatch logic lives +in ``espp::detail::OdriveNativeCore``, a host-buildable wire core that depends +only on the C++ standard library, so the protocol can be unit-tested off-target. + +Features +-------- + +- Transport-agnostic: one packet in via ``process_bytes``, one response packet out +- Typed property registry: ``register_float_property`` plus signed/unsigned 8/16/32/64-bit + integer and ``bool`` variants (no exceptions; uses ``std::error_code``) +- Auto-discovery: builds the endpoint-0 JSON descriptor and ``json_crc`` +- Thread-safe; user getters/setters are never invoked while a lock is held +- No direct hardware dependencies; uses ``std::function`` for DI + +Basic Usage +----------- + +.. code-block:: cpp + + espp::OdriveNative proto({.log_level = espp::Logger::Verbosity::INFO}); + float vbus = 24.0f, input_pos = 0.0f; + proto.register_float_property("vbus_voltage", [&]() { return vbus; }); + proto.register_float_property("axis0.controller.input_pos", + [&]() { return input_pos; }, + [&](float v, std::error_code &ec) { input_pos = v; ec.clear(); return true; }); + + // One USB bulk transfer == one packet. + auto resp = proto.process_bytes(std::span(rx_buf, rx_len)); + // Transmit resp back over the same transport (empty when no response expected) + +Protocol +-------- + +The authoritative wire specification (packet format, CRC-16 with poly 0x3d65 / +init 0x1337, endpoint dispatch, little-endian type codecs, and the compact JSON +schema) is documented in ``components/odrive_native/PROTOCOL.md``. + +Notes +----- + +This component implements the property (primitive get/set) surface of the legacy +protocol; functions / endpoint refs are not implemented yet. Wiring to a concrete +USB device stack is handled in a later phase. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + odrive_native_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/odrive_native.inc diff --git a/doc/en/motor_control/odrive_native_example.md b/doc/en/motor_control/odrive_native_example.md new file mode 100644 index 000000000..1335a5b2e --- /dev/null +++ b/doc/en/motor_control/odrive_native_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/odrive_native/example/README.md +``` diff --git a/lib/espp.cmake b/lib/espp.cmake index 76afcd379..10a965849 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -101,6 +101,7 @@ set(ESPP_INCLUDES ${ESPP_COMPONENTS}/logger/include ${ESPP_COMPONENTS}/math/include ${ESPP_COMPONENTS}/ndef/include + ${ESPP_COMPONENTS}/odrive_native/include ${ESPP_COMPONENTS}/pid/include ${ESPP_COMPONENTS}/rtps/include ${ESPP_COMPONENTS}/rtsp/include @@ -192,6 +193,7 @@ set(ESPP_PYTHON_BINDINGS_DIR ${CMAKE_CURRENT_LIST_DIR}/python_bindings) set(ESPP_PYTHON_SOURCES ${ESPP_PYTHON_BINDINGS_DIR}/module.cpp ${ESPP_PYTHON_BINDINGS_DIR}/pybind_espp.cpp + ${ESPP_PYTHON_BINDINGS_DIR}/odrive_native_bindings.cpp ${ESPP_PYTHON_BINDINGS_DIR}/rtps_bindings.cpp ${ESPP_PYTHON_BINDINGS_DIR}/socket_reactor_bindings.cpp ${ESPP_SOURCES} @@ -290,6 +292,13 @@ function(espp_install_python_module) DESTINATION . PATTERN "__pycache__" EXCLUDE PATTERN ".mypy_cache" EXCLUDE) + # Also stage espp_odrive (the pure-python ODrive client that lives in the + # odrive_native component) next to the espp package, mirroring the wheel's + # `wheel.packages` in pyproject.toml -- so PYTHONPATH= gives both + # `import espp` and `import espp_odrive` (and espp.odrive works). + install(DIRECTORY ${ESPP_COMPONENTS}/odrive_native/python/espp_odrive + DESTINATION . + PATTERN "__pycache__" EXCLUDE) install(TARGETS _espp LIBRARY DESTINATION espp/ RUNTIME DESTINATION espp/) diff --git a/lib/include/espp.hpp b/lib/include/espp.hpp index f4c6d0de0..9e9cdd825 100644 --- a/lib/include/espp.hpp +++ b/lib/include/espp.hpp @@ -42,6 +42,7 @@ extern "C" { #include "mjpeg_depacketizer.hpp" #include "mjpeg_packetizer.hpp" #include "ndef.hpp" +#include "odrive_native.hpp" #include "pid.hpp" #include "range_mapper.hpp" #include "rtp_depacketizer.hpp" diff --git a/lib/python_bindings/espp/__init__.py b/lib/python_bindings/espp/__init__.py index db5ad3445..1743a1136 100644 --- a/lib/python_bindings/espp/__init__.py +++ b/lib/python_bindings/espp/__init__.py @@ -11,3 +11,13 @@ # Pure-Python typed pub/sub layer over the bound RtpsParticipant (accessible as # ``espp.rtps.Publisher`` / ``espp.rtps.Subscriber``). from . import rtps # noqa: F401,E402 + +# The pure-python ODrive legacy-native protocol CLIENT (espp_odrive, sourced +# from components/odrive_native/python) ships alongside this package in the +# wheel / installed prefix; expose it as ``espp.odrive`` for convenience. The +# bound C++ SERVER side is ``espp.OdriveNative`` (from _espp above). Optional: +# a source-tree espp package without the sibling won't have the alias. +try: + import espp_odrive as odrive # noqa: F401,E402 +except ImportError: # pragma: no cover - sibling package not on the path + pass diff --git a/lib/python_bindings/module.cpp b/lib/python_bindings/module.cpp index 365cbf152..60c413669 100644 --- a/lib/python_bindings/module.cpp +++ b/lib/python_bindings/module.cpp @@ -16,6 +16,10 @@ void py_init_rtps(py::module &m); // socket_reactor_bindings.cpp). Runs after py_init_module_espp so UdpSocket / Socket::Info / // Logger::Verbosity are already registered. void py_init_socket_reactor(py::module &m); +// Hand-written bindings for espp::OdriveNative + the fibre stream framing helpers (std::function +// accessors with std::error_code& and std::span wire APIs; see odrive_native_bindings.cpp). Runs +// after py_init_module_espp so Logger::Verbosity is already registered. +void py_init_odrive_native(py::module &m); // This builds the native python extension module `espp._espp`, which the // `espp` python package (python_bindings/espp/__init__.py) re-exports. @@ -29,4 +33,5 @@ PYBIND11_MODULE(_espp, m) { py_init_module_espp(m); py_init_rtps(m); py_init_socket_reactor(m); + py_init_odrive_native(m); } diff --git a/lib/python_bindings/odrive_native_bindings.cpp b/lib/python_bindings/odrive_native_bindings.cpp new file mode 100644 index 000000000..711e581d6 --- /dev/null +++ b/lib/python_bindings/odrive_native_bindings.cpp @@ -0,0 +1,188 @@ +// Hand-written pybind11 bindings for espp::OdriveNative (the transport-agnostic +// ODrive legacy native / Fibre endpoint protocol server in +// components/odrive_native) plus its stream-framing helpers. +// +// Why hand-written (like rtps/cdr): the registration API takes std::function +// getters/setters with std::error_code& out-params and the wire API uses +// std::span — neither of which litgen can bind generically. The +// shim exposes a pythonic API instead: +// dev = espp.OdriveNative() +// dev.register_float_property("axis0.controller.input_pos", +// getter=lambda: pos, setter=on_set) # setter optional +// resp = dev.process_bytes(request_bytes) # bytes -> bytes (empty = no response) +// dev.json(), dev.json_crc() +// plus module-level odrive_crc16 / odrive_crc8 / odrive_stream_frame and an +// OdriveStreamDeframer class for the UART stream framing. +// +// GIL notes: process_bytes() is called from Python (GIL held) and invokes the +// registered getters/setters synchronously on the same thread, so callbacks are +// simply invoked under the caller's GIL. The gil_scoped_acquire in the wrappers +// is a cheap no-op there and keeps the callbacks safe if a future embedder +// calls process_bytes() from a non-Python thread. +// +// It is kept out of the generated pybind_espp.cpp so regeneration never +// clobbers it. + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "detail/odrive_native_stream.hpp" // stream framing + CRC8 (not pulled in by the public header) +#include "odrive_native.hpp" + +namespace py = pybind11; + +namespace { + +py::bytes vec_to_bytes(const std::vector &v) { + return py::bytes(reinterpret_cast(v.data()), v.size()); +} + +std::vector bytes_to_vec(const py::bytes &b) { + std::string s = b; // pybind copies the buffer + return std::vector(s.begin(), s.end()); +} + +// Define one register__property binding. The Python getter is a +// zero-argument callable returning the value; the optional setter is called +// with the new value and may return False (or raise) to reject the write — +// a rejected write is reported through the component's error callback (the +// logger by default) since the wire protocol has no error channel. +template +void def_register(py::class_ &cls, const char *name, RegFn reg) { + cls.def( + name, + [reg](espp::OdriveNative &self, const std::string &path, const py::function &getter, + const py::object &setter) { + std::function g = nullptr; + if (!getter.is_none()) { + g = [getter]() -> T { + py::gil_scoped_acquire gil; + return getter().template cast(); + }; + } + std::function s = nullptr; + if (!setter.is_none()) { + auto sf = setter.cast(); + s = [sf](T v, std::error_code &ec) -> bool { + py::gil_scoped_acquire gil; + try { + py::object r = sf(v); + const bool ok = r.is_none() ? true : r.cast(); + if (!ok) + ec = std::make_error_code(std::errc::invalid_argument); + return ok; + } catch (const py::error_already_set &) { + ec = std::make_error_code(std::errc::invalid_argument); + return false; + } + }; + } + (self.*reg)(path, g, s); + }, + py::arg("path"), py::arg("getter"), py::arg("setter") = py::none()); +} + +} // namespace + +void py_init_odrive_native(py::module &m) { + using espp::OdriveNative; + using espp::detail::odrive_crc16; + using espp::detail::odrive_crc8; + using espp::detail::stream_frame; + using espp::detail::StreamDeframer; + + // ---- CRC + UART stream-framing helpers --------------------------------- + m.def( + "odrive_crc16", + [](const py::bytes &data, uint16_t init) { + auto v = bytes_to_vec(data); + return odrive_crc16(std::span(v.data(), v.size()), init); + }, + py::arg("data"), py::arg("init") = 0x1337, + "ODrive legacy CRC-16 (poly 0x3d65). init=0x1337 for packet/stream use; " + "pass init=1 (PROTOCOL_VERSION) to compute a json_crc."); + m.def( + "odrive_crc8", + [](const py::bytes &data, uint8_t init) { + auto v = bytes_to_vec(data); + return odrive_crc8(std::span(v.data(), v.size()), init); + }, + py::arg("data"), py::arg("init") = 0x42, "ODrive/fibre stream CRC-8 (poly 0x37, init 0x42)."); + m.def( + "odrive_stream_frame", + [](const py::bytes &packet) { + auto v = bytes_to_vec(packet); + return vec_to_bytes(stream_frame(std::span(v.data(), v.size()))); + }, + py::arg("packet"), + "Wrap one packet (< 128 bytes) in the fibre UART stream framing " + "(sync, len, crc8, packet, crc16-BE). Returns b'' if the packet is too large."); + + py::class_(m, "OdriveStreamDeframer", + "Stateful deframer for the fibre UART stream framing: feed received " + "chunks to push() and get back complete, CRC-verified packets.") + .def(py::init<>()) + .def( + "push", + [](StreamDeframer &self, const py::bytes &data) { + auto v = bytes_to_vec(data); + auto packets = self.push(std::span(v.data(), v.size())); + std::vector out; + out.reserve(packets.size()); + for (const auto &p : packets) + out.push_back(vec_to_bytes(p)); + return out; + }, + py::arg("data"), "Append received stream bytes; returns the list of decoded packets.") + .def("buffered", &StreamDeframer::buffered, + "Bytes currently buffered awaiting a complete frame."); + + // ---- The protocol server ------------------------------------------------ + auto cls = + py::class_(m, "OdriveNative", + "Transport-agnostic server for the ODrive legacy native (Fibre " + "endpoint) binary protocol. Register typed properties from dotted " + "paths, then feed request packets to process_bytes() and send back " + "whatever it returns (empty = no response expected).") + .def(py::init([](espp::Logger::Verbosity log_level) { + return new OdriveNative(OdriveNative::Config{.log_level = log_level}); + }), + py::arg("log_level") = espp::Logger::Verbosity::WARN) + .def( + "process_bytes", + [](OdriveNative &self, const py::bytes &data) { + auto v = bytes_to_vec(data); + return vec_to_bytes( + self.process_bytes(std::span(v.data(), v.size()))); + }, + py::arg("data"), + "Process exactly one inbound request packet; returns the response packet " + "bytes (empty when no response is expected / the packet is ignored).") + .def("finalize", &OdriveNative::finalize, + "Build (or rebuild) the JSON descriptor + CRC now (otherwise lazy).") + .def( + "json", [](OdriveNative &self) { return self.json(); }, + "The compact JSON endpoint descriptor (endpoint 0 blob).") + .def( + "json_crc", [](OdriveNative &self) { return self.json_crc(); }, + "CRC-16 of the JSON descriptor (the canary for non-zero endpoints)."); + + def_register(cls, "register_float_property", &OdriveNative::register_float_property); + def_register(cls, "register_int8_property", &OdriveNative::register_int8_property); + def_register(cls, "register_uint8_property", &OdriveNative::register_uint8_property); + def_register(cls, "register_int16_property", &OdriveNative::register_int16_property); + def_register(cls, "register_uint16_property", &OdriveNative::register_uint16_property); + def_register(cls, "register_int32_property", &OdriveNative::register_int32_property); + def_register(cls, "register_uint32_property", &OdriveNative::register_uint32_property); + def_register(cls, "register_int64_property", &OdriveNative::register_int64_property); + def_register(cls, "register_uint64_property", &OdriveNative::register_uint64_property); + def_register(cls, "register_bool_property", &OdriveNative::register_bool_property); +} diff --git a/pc/tests/odrive_native_golden.cpp b/pc/tests/odrive_native_golden.cpp new file mode 100644 index 000000000..3a8fe2d77 --- /dev/null +++ b/pc/tests/odrive_native_golden.cpp @@ -0,0 +1,194 @@ +// Golden wire-format test for the ODrive legacy native (Fibre endpoint) protocol +// -- the analogue of rtps_golden for components/odrive_native. It freezes, byte +// for byte, the CRC8 / CRC16 constants, the UART stream frame, and a packet +// round-trip, all verified against the fw-v0.5.1 reference (and proven end-to-end +// by the serial-loopback interop harness in components/odrive_native/interop). +// +// Built by the pc harness against the installed espp package: odrive_native is +// part of the x-platform espp lib, so its headers come from the espp::espp +// target like every other test here. Exits 0 when every golden matches. + +#include +#include +#include +#include +#include +#include +#include + +#include "detail/odrive_native_core.hpp" +#include "detail/odrive_native_stream.hpp" +#include "odrive_native.hpp" + +using espp::detail::kProtocolVersion; +using espp::detail::odrive_crc16; +using espp::detail::odrive_crc8; +using espp::detail::OdriveNativeCore; +using espp::detail::stream_frame; +using espp::detail::StreamDeframer; + +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 std::span sv(const char *s) { + return std::span(reinterpret_cast(s), std::strlen(s)); +} + +static void golden_crc8() { + std::printf("golden_crc8\n"); + CHECK(odrive_crc8(std::span{}) == 0x42); // init + const uint8_t z = 0x00; + CHECK(odrive_crc8(std::span(&z, 1)) == 0xca); + CHECK(odrive_crc8(sv("123456789")) == 0x8c); + const uint8_t hdr[2] = {0xAA, 0x0A}; + CHECK(odrive_crc8(std::span(hdr, 2)) == 0x53); +} + +static void golden_crc16() { + std::printf("golden_crc16\n"); + // Packet-CRC init (0x1337) -- the UART stream framing / packet trailer of ep 0. + CHECK(odrive_crc16(std::string_view("")) == 0x1337); + const uint8_t z = 0x00; + CHECK(odrive_crc16(std::span(&z, 1)) == 0xe150); + CHECK(odrive_crc16(std::string_view("123456789")) == 0xaa01); +} + +static void golden_frame() { + std::printf("golden_frame\n"); + const std::vector packet = {0x80, 0x80, 0x00, 0x80, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00}; + const std::vector expected = {0xAA, 0x0C, 0xE1, 0x80, 0x80, 0x00, 0x80, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0xA3, 0xAB}; + CHECK(stream_frame(packet) == expected); + + // Deframe round-trip (with the CRC16 trailer stripped back to the raw packet). + StreamDeframer d; + auto pkts = d.push(expected); + CHECK(pkts.size() == 1); + if (pkts.size() == 1) + CHECK(pkts[0] == packet); +} + +static void golden_packet_roundtrip() { + std::printf("golden_packet_roundtrip\n"); + OdriveNativeCore core; + float pos = 0.0f; + core.register_float_property("vbus_voltage", [] { return 24.0f; }); + core.register_float_property( + "axis0.controller.input_pos", [&] { return pos; }, + [&](float v, std::error_code &ec) { + pos = v; + ec.clear(); + return true; + }); + + // The endpoint canary is CRC16 over the JSON seeded with PROTOCOL_VERSION (1), + // NOT the 0x1337 packet-CRC init -- this is what a real fibre client sends. + const std::string json = core.json(); + CHECK(core.json_crc() == odrive_crc16(json, kProtocolVersion)); + CHECK(core.json_crc() != odrive_crc16(json)); // and it differs from the 0x1337 init + + // Full frame -> deframe -> process -> reframe -> deframe path for a value write + // then read-back of endpoint 2 (input_pos), the exact loop the device shim runs. + const uint16_t crc = core.json_crc(); + const uint16_t ep = 2; + const float wrote = 12.5f; + std::vector req; // [seq][ep|0x8000][out_len][payload][trailer] + auto put16 = [&](std::vector &v, uint16_t x) { + v.push_back(uint8_t(x & 0xff)); + v.push_back(uint8_t(x >> 8)); + }; + put16(req, 0x0080 | 0x0001); + put16(req, ep | 0x8000); + put16(req, 4); // want 4 bytes back + uint8_t fb[4]; + std::memcpy(fb, &wrote, 4); + req.insert(req.end(), fb, fb + 4); + put16(req, crc); + + auto framed = stream_frame(req); + StreamDeframer d; + auto in = d.push(framed); + CHECK(in.size() == 1); + auto resp = core.process_bytes(in[0]); + CHECK(resp.size() == 2 + 4); + float rb = 0.0f; + std::memcpy(&rb, resp.data() + 2, 4); + CHECK(rb == wrote); + + auto resp_framed = stream_frame(resp); + StreamDeframer d2; + auto rp = d2.push(resp_framed); + CHECK(rp.size() == 1); + if (rp.size() == 1) + CHECK(rp[0] == resp); +} + +// The espp::OdriveNative wrapper (BaseComponent + the core) is what the lib +// exposes; exercise it through the same read/write path to prove the +// x-platform packaging end-to-end (not just the detail/ headers). +static void lib_wrapper_roundtrip() { + std::printf("lib_wrapper_roundtrip\n"); + espp::OdriveNative dev(espp::OdriveNative::Config{.log_level = espp::Logger::Verbosity::WARN}); + float pos = 1.5f; + dev.register_float_property( + "axis0.controller.input_pos", [&]() { return pos; }, + [&](float v, std::error_code &ec) { + ec.clear(); + pos = v; + return true; + }); + const uint16_t crc = dev.json_crc(); + CHECK(crc != 0); + + // write 42.0f to endpoint 1 (expect a 2-byte ack), then read it back + const float wrote = 42.0f; + std::vector req; + auto put_u16 = [&](uint16_t x) { + req.push_back(uint8_t(x & 0xff)); + req.push_back(uint8_t((x >> 8) & 0xff)); + }; + put_u16(0x0001); // seq + put_u16(0x8000 | 1); // endpoint 1, expect response + put_u16(0); // output_len + uint8_t fb[4]; + std::memcpy(fb, &wrote, 4); + req.insert(req.end(), fb, fb + 4); + put_u16(crc); // trailer canary + auto ack = dev.process_bytes(req); + CHECK(ack.size() == 2); + CHECK(pos == wrote); + + req.clear(); + put_u16(0x0002); + put_u16(0x8000 | 1); + put_u16(4); + put_u16(crc); + auto resp = dev.process_bytes(req); + CHECK(resp.size() == 2 + 4); + if (resp.size() == 6) { + float rb = 0; + std::memcpy(&rb, resp.data() + 2, 4); + CHECK(rb == wrote); + } +} + +int main() { + golden_crc8(); + golden_crc16(); + golden_frame(); + golden_packet_roundtrip(); + lib_wrapper_roundtrip(); + if (g_failures == 0) { + std::printf("\nODRIVE_NATIVE GOLDEN: ALL PASSED\n"); + return 0; + } + std::printf("\nODRIVE_NATIVE GOLDEN: %d CHECK(S) FAILED\n", g_failures); + return 1; +} diff --git a/pyproject.toml b/pyproject.toml index 327535f91..57cb983e3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ license = "MIT" license-files = ["LICENSE"] authors = [{ name = "William Emfinger", email = "waemfinger@gmail.com" }] requires-python = ">=3.10" -keywords = ["espp", "esp-cpp", "embedded", "rtsp", "rtps", "cdr", "cobs", "sockets"] +keywords = ["espp", "esp-cpp", "embedded", "rtsp", "rtps", "cdr", "cobs", "sockets", "odrive"] classifiers = [ "Development Status :: 4 - Beta", "Intended Audience :: Developers", @@ -30,13 +30,25 @@ Documentation = "https://esp-cpp.github.io/espp/" Repository = "https://github.com/esp-cpp/espp" Issues = "https://github.com/esp-cpp/espp/issues" +[project.optional-dependencies] +# espp_odrive's serial transport imports pyserial lazily; everything else in +# the shipped packages is stdlib-only. `pip install espp[serial]` enables the +# espp_odrive serial/UART transports. +serial = ["pyserial"] + [tool.scikit-build] # The CMake project for the host (PC) build lives in lib/; the repo root is the # project root so the sdist can include the components/ sources it references. cmake.source-dir = "lib" # Pure-python part of the package (espp/__init__.py, __init__.pyi, py.typed); -# the compiled espp._espp extension is installed into it by CMake. -wheel.packages = ["lib/python_bindings/espp"] +# the compiled espp._espp extension is installed into it by CMake. The wheel +# ALSO ships `espp_odrive`, the pure-python ODrive legacy-native protocol +# client whose single source of truth lives in the odrive_native component -- +# stdlib-only except a lazily-imported pyserial (see the `serial` extra). +wheel.packages = [ + "lib/python_bindings/espp", + "components/odrive_native/python/espp_odrive", +] wheel.exclude = ["**/.mypy_cache", "**/__pycache__"] # Persistent CMake build dir (gitignored) so repeated local builds - notably # re-running `pip install -e .` after a C++ change - are incremental. @@ -46,7 +58,9 @@ sdist.exclude = [ "doc/", "docs/", "pc/", - "python/", + # anchored to the repo root: components/odrive_native/python/ (the shipped + # espp_odrive package) must stay IN the sdist + "/python/", # local build helpers / outputs (install/, build/, _build/ already gitignored) "*.sh", "*.ps1", diff --git a/python/odrive_native_test.py b/python/odrive_native_test.py new file mode 100644 index 000000000..2110ca458 --- /dev/null +++ b/python/odrive_native_test.py @@ -0,0 +1,165 @@ +"""OdriveNative Python-binding test. + +In-process loopback between the bound C++ protocol server (espp.OdriveNative, +the same wire core the firmware runs) and the pure-python espp_odrive client +helpers (components/odrive_native/python) -- the packets built by the client +codec are fed straight into the server's process_bytes(), which is exactly +what travels over USB/UART on hardware. Also freezes the CRC goldens and the +UART stream framing through the bindings. + +Exit code 0 on full pass, 1 on any failure. +""" + +import json +import os +import struct +import sys +from typing import List, Tuple + +import espp + +# The pure-python ODrive client codec (espp_odrive) ships alongside the espp +# package in the wheel / installed prefix; fall back to its source-of-truth +# location in the odrive_native component for source-tree runs. +import importlib.util + +if importlib.util.find_spec("espp_odrive") is None: + sys.path.insert( + 0, + os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "components", + "odrive_native", "python")) +from espp_odrive import PROTOCOL_VERSION, crc8, crc16 # noqa: E402 +from espp_odrive.protocol import build_packet, parse_response # noqa: E402 + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +results: List[Tuple[str, bool]] = [] + + +def check(test: str, condition: bool, desc: str) -> bool: + if condition: + print(f" PASS [{test}]: {desc}") + else: + print(f" FAIL [{test}]: {desc}") + results.append((test, condition)) + return condition + + +# --------------------------------------------------------------------------- +# 1. CRC goldens: bindings vs the pure-python client codec +# --------------------------------------------------------------------------- +print("1. CRC goldens (bindings vs espp_odrive)") +check("crc", espp.odrive_crc16(b"123456789") == 0xAA01, "crc16 golden 0xaa01") +check("crc", espp.odrive_crc8(b"123456789") == 0x8C, "crc8 golden 0x8c") +check("crc", espp.odrive_crc16(b"123456789") == crc16(b"123456789"), + "crc16 matches espp_odrive.crc16") +check("crc", espp.odrive_crc8(b"123456789") == crc8(b"123456789"), + "crc8 matches espp_odrive.crc8") + +# --------------------------------------------------------------------------- +# 2. Server construction + property registration from Python +# --------------------------------------------------------------------------- +print("2. Server + registration") +state = {"vbus": 24.0, "pos": 0.0, "error": 0, "locked": 7.0} + +dev = espp.OdriveNative() +dev.register_float_property("vbus_voltage", lambda: state["vbus"]) +dev.register_float_property( + "axis0.controller.input_pos", lambda: state["pos"], + lambda v: state.__setitem__("pos", v) or True) +dev.register_uint32_property("axis0.error", lambda: state["error"]) +# a setter that REJECTS every write (returns False) +dev.register_float_property("axis0.locked", lambda: state["locked"], lambda v: False) + +descriptor = dev.json() +jcrc = dev.json_crc() +check("reg", jcrc != 0, f"json_crc nonzero (0x{jcrc:04x})") +check("reg", jcrc == crc16(descriptor.encode("ascii"), PROTOCOL_VERSION), + "json_crc == espp_odrive crc16(json, init=PROTOCOL_VERSION)") + +tree = json.loads(descriptor) +check("reg", isinstance(tree, list) and len(tree) > 0, "descriptor parses as JSON") + + +def find_endpoint(nodes, dotted): + parts = dotted.split(".") + for part in parts[:-1]: + nodes = next(n for n in nodes if n["name"] == part)["members"] + return next(n for n in nodes if n["name"] == parts[-1]) + + +ep_pos = find_endpoint(tree, "axis0.controller.input_pos") +ep_locked = find_endpoint(tree, "axis0.locked") +check("reg", ep_pos["access"] == "rw", "input_pos advertises rw") + +# --------------------------------------------------------------------------- +# 3. Endpoint-0 chunked JSON download through the wire path +# --------------------------------------------------------------------------- +print("3. Endpoint-0 JSON download") +blob = b"" +for _ in range(64): + req = build_packet(1, 0, 512, struct.pack(" value unchanged +req = build_packet(4, ep_locked["id"], 0, struct.pack(" request ignored entirely +req = build_packet(5, ep_pos["id"], 4, b"", True, jcrc ^ 0xFFFF) +check("rw", dev.process_bytes(req) == b"", "canary mismatch is ignored") + +# unknown endpoint -> ignored even with the expect-response bit +req = build_packet(6, 999, 4, b"", True, jcrc) +check("rw", dev.process_bytes(req) == b"", "unknown endpoint is ignored") + +# --------------------------------------------------------------------------- +# 5. UART stream framing helpers +# --------------------------------------------------------------------------- +print("5. Stream framing") +pkt = build_packet(7, ep_pos["id"], 4, b"", True, jcrc) +framed = espp.odrive_stream_frame(pkt) +check("stream", len(framed) == len(pkt) + 5, "frame adds sync+len+crc8+crc16") +deframer = espp.OdriveStreamDeframer() +mid = len(framed) // 2 +out = deframer.push(framed[:mid]) +check("stream", out == [], "partial frame yields nothing") +out = deframer.push(framed[mid:]) +check("stream", out == [pkt], "completed frame yields the original packet") +check("stream", deframer.buffered() == 0, "deframer drained") +check("stream", espp.odrive_stream_frame(b"\x00" * 200) == b"", + "oversize packet (>127) refused") + +# --------------------------------------------------------------------------- +# Summary +# --------------------------------------------------------------------------- +failed = [t for t, ok in results if not ok] +print(f"\n{len(results) - len(failed)}/{len(results)} checks passed") +if failed: + print("FAILED:", ", ".join(sorted(set(failed)))) + sys.exit(1) +print("ODRIVE_NATIVE PYTHON BINDINGS: ALL PASSED") +sys.exit(0)