diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index dbe2e26..450e780 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -120,30 +120,43 @@ jobs: # credential. Configure a read-only deploy key or a fine-grained PAT # scoped to that one repo as the FIRMWARE_REPO_TOKEN secret. # - # The secret is deliberately optional. It is absent on pull requests from - # forks (GitHub withholds secrets there by design), so the check degrades - # to a loud SKIP instead of failing work it cannot verify. Real coverage - # comes from pushes to main and from PRs opened inside the repo. - - name: Fetch firmware reference headers + # Fork PRs do not receive the private-repo credential. On trusted runs + # a missing credential is a configuration error, not a clean mirror. + - name: Check firmware access id: firmware env: TOKEN: ${{ secrets.FIRMWARE_REPO_TOKEN }} + FORK_PR: ${{ github.event_name == 'pull_request' && github.event.pull_request.head.repo.full_name != github.repository }} run: | if [ -z "$TOKEN" ]; then - echo "::notice::FIRMWARE_REPO_TOKEN unavailable - drift check will SKIP" + if [ "$FORK_PR" != true ]; then + echo "::error::FIRMWARE_REPO_TOKEN is required for protocol comparison" + exit 1 + fi + echo "::notice::Private firmware headers unavailable to fork PR" echo "available=false" >> "$GITHUB_OUTPUT" exit 0 fi - # Only the protocol headers are needed, so keep the clone shallow. - git clone --depth 1 --branch hw_v1.1.0 \ - "https://x-access-token:${TOKEN}@github.com/XenseRobotics-AI/tc-gu-01.git" \ - third_party/firmware/tc-gu-01 echo "available=true" >> "$GITHUB_OUTPUT" + # #29 introduces the wire protocol together with firmware PR #23. + # All other runs, including pushes after merge, track the firmware base. + - name: Fetch firmware reference headers + if: steps.firmware.outputs.available == 'true' + uses: actions/checkout@v5 + with: + repository: XenseRobotics-AI/tc-gu-01 + ref: ${{ github.event_name == 'pull_request' && github.event.pull_request.number == 29 && 'refs/pull/23/head' || 'hw_v1.1.0' }} + path: third_party/firmware/tc-gu-01 + token: ${{ secrets.FIRMWARE_REPO_TOKEN }} + persist-credentials: false + sparse-checkout: | + App/protocol + App/drivers + - name: Check protocol mirror - run: | - python3 scripts/check_protocol_drift.py \ - ${{ steps.firmware.outputs.available == 'true' && '--require' || '' }} + if: steps.firmware.outputs.available == 'true' + run: python3 scripts/check_protocol_drift.py --require # Leaves nothing on disk that a later step could publish; the clone lives # in a gitignored path, but be explicit rather than rely on that. diff --git a/CHANGELOG.md b/CHANGELOG.md index 87f0555..f45bb9a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,105 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Changed + +- **Bundled firmware is now leader 1.2.7 / follower 1.2.17 (protocol V2.9).** + Both add time-shared exchange (`0x5E` leader, `0x5D` follower) so host and + gripper never transmit at the same time; an uplink frame that overlaps a host + byte loses the bytes in flight. **Host-breaking:** IMU/encoder `timestamp` moves + from DWT/250 (wraps every 17.18 s) to TIM2 1 MHz (wraps every 71 min 35 s); this + SDK already unwraps the new timebase, older host software does not. Images are + built with arm-none-eabi-gcc 10.3.1 from tc-gu-01 `8c059e9`; size, CRC32 and + notes are in `firmware/manifest.json`. Leader verified on the collector backpack + (0 bad uplink frames under load vs 17/7208 on 1.2.6 push). As before, replug the + leader's USB / power-cycle the follower's 24 V after flashing. + +### Fixed + +- **A lost follower exchange reply no longer loses IMU samples.** The follower + exchange sent each request once; when its reply was lost (host stalled past + the timeout) the next exchange took new samples and the ones in the lost reply + were gone (5-9 per hour on a loaded host). It now retries with the same seq, + like the leader, so the firmware replays its cached reply. + +- **A wall-clock step no longer stalls the exchange or the controllers.** + libstdc++ built against an older glibc sysroot (conda-forge, manylinux) + implements `condition_variable::wait_until(steady_clock)` and + `timed_mutex::try_lock_for` on `CLOCK_REALTIME`, so when NTP stepped the wall + clock back the waits lasted that much longer. A follower exchange stalled + past `status_timeout_ms` 0.2 s after "Clock change detected" and the + ForcePositionController faulted. The leader/follower exchange loops, the + half-duplex turn lock and both controllers' status watchdog now wait on a + `pthread_cond` bound to `CLOCK_MONOTONIC` (`taccap/detail/monotonic_cv.hpp`), + whatever toolchain builds the SDK. + +- **`LeaderGripper.start_exchange(1000, 100)` no longer throws.** With no + `exchange_hz` the default exchange rate was the requested data rate, so a + 1000 Hz IMU request asked for 1000 exchanges/s and was rejected (limit 500). + The default is now the data rate capped at 200 Hz (the leader samples IMU and + encoder at 100 Hz whatever is requested); an explicit `exchange_hz` may still + be up to 500. + +- **Python discovery no longer freezes other threads.** `scan_grippers()` and + the `find_*()` helpers held the GIL for the whole scan (~280 ms with two idle + grippers attached, up to the ACK timeout on a silent port), so a GUI scanning + on a background thread stalled its own 100 Hz target thread for the same + time. The PC config tool rescans every 3 s while one role's gripper is held + by another process, which made reciprocation stutter every 3 s whenever the + leader was in use elsewhere. The bindings now release the GIL; measured + stall during a scan: 263-280 ms before, 0 ms after. + +### Added + +- **`LeaderGripper.start_streaming(imu_hz, encoder_hz, imu_v2_time=True)`.** + `imu_v2_time=False` streams the legacy 28 B IMU frame without a time session, + as SDK <= 0.4.4 did. A pushed stream loses an uplink frame whenever a host + write overlaps it, and a longer frame is hit more often: on a leader at + 100 Hz queries (IMU 1000 Hz requested, encoder 100 Hz, 120 s x3) IMU bad + frames were 868 with V2 and 1 with the legacy frame. Default unchanged; + `start_exchange()` removes the overlap and keeps V2. + +- **Leader half-duplex exchange: `LeaderGripper.start_exchange(imu_hz, + encoder_hz, exchange_hz, fallback_to_streaming=True)`.** Requires leader + firmware with `StreamMode::Polled` and `Cmd::LeaderExchange` (0x5E, tc-gu-01 + `docs/LEADER_EXCHANGE.md`). The leader holds IMU, encoder, key and + sensor-error frames instead of pushing them; the SDK fetches them (again at + once while a backlog remains) and delivers them to the usual subscribers, + and every request on the link takes turns, so host and leader never + transmit at the same time. A lost reply is retried with the same seq, so the + firmware replays it and no records are lost. On firmware without 0x5E it + falls back to `start_streaming()` and returns `False` (or raises + `ProtocolError` with `fallback_to_streaming=False`); the device version is + not used to decide, since it stays 1.2.6. `exchange_stats()` returns + `LeaderExchangeStats`. `start_streaming()` and the wire protocol of older + commands are unchanged. + +- **Leader exchange survives the firmware lease.** Leader firmware leaves the + exchange by itself after 1 s without `Cmd::LeaderExchange` (host crashed or + stalled), flushes what it held as ordinary DATA frames and goes back to + pushing. A reply without `LeaderExchangeRespFlag::Polled` now makes the SDK + re-send the polled `StartStream` (at most every 200 ms), so a stalled host + returns to the exchange with nothing lost; `LeaderExchangeStats` gains + `rearms` and `rearm_failures`. `start_exchange` documents `exchange_hz` + (2x the data rate removes the occasional one-period wait) and that samples + from one reply share its arrival time as `host_time`. + + Known limit: the half-duplex turn lock is not fair. A user thread issuing + requests back-to-back with no pause can delay exchanges (seen in a test at + ~10 kHz of heartbeats); health checks at realistic rates (<= 1 kHz) are + unaffected. + +- **Follower half-duplex exchange: `FollowerGripper.start_exchange(motor_hz, + imu_hz, exchange_hz)`.** Requires follower firmware with `StreamMode::Polled` + and `Cmd::SlaveExchange` (0x5D, tc-gu-01 `docs/HALF_DUPLEX_EXCHANGE.md`). + The follower stops pushing frames; the SDK polls it and delivers motor + status / IMU to the usual subscribers, `Motor.submit_*()` targets ride on the + exchange, and every request on the link takes turns, so host and follower + never transmit at the same time. Existing controllers run unchanged on top. + `exchange_stats()` reports timeouts, resent and rejected targets and round + trips. Older firmware raises `ProtocolError` and nothing is left running. + `bus::RetryMode::Once` sends exactly one attempt. + ## [0.4.4] - 2026-11-01 _The `v0.4.4` tag was moved on 2026-11-02 to include the tactile marker-flow diff --git a/README.md b/README.md index c530d95..ef8df66 100644 --- a/README.md +++ b/README.md @@ -364,7 +364,7 @@ motor's own firmware, checked against the required minimum. It is read-only: ```bash python python/examples/follower_status.py left -# [fw] FirmwareVersion(1.2.16.0) +# [fw] FirmwareVersion(1.2.17.0) # [motor] MotorModel(RS00, id=1, from flash, t_max=14.0Nm, v_max=33.0rad/s) # [motor fw] 0.0.3.32 (...) OK ``` diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 3c681ea..c76fa27 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -20,6 +20,8 @@ add_library(taccap_core SHARED src/discovery.cpp src/leader_gripper.cpp src/follower_gripper.cpp + src/follower_exchange.cpp + src/leader_exchange.cpp src/force_position_controller.cpp src/impedance_controller.cpp src/ota.cpp diff --git a/cpp/include/taccap/bus/frame.hpp b/cpp/include/taccap/bus/frame.hpp index b714191..78130ea 100644 --- a/cpp/include/taccap/bus/frame.hpp +++ b/cpp/include/taccap/bus/frame.hpp @@ -16,6 +16,10 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -58,6 +62,9 @@ struct Frame { protocol::FrameType type; protocol::Cmd cmd; std::vector payload; + // Host monotonic time of the read completing this frame, not callback time + // or hardware sample time. Zero for standalone parser-created frames. + std::chrono::steady_clock::time_point receive_time{}; }; // Build a wire frame. @@ -90,9 +97,10 @@ enum class ParseStatus { Success, NeedMoreData, Resync }; // status == Resync; Success / NeedMoreData always carry ParseReason::None. // // This exists to separate "we are mid-stream on a byte boundary" from "we -// found a well-formed frame whose CRC is wrong". The latter is the signature -// of *lost bytes* — a host that stopped draining the tty long enough for the -// kernel/USB buffers to overflow — and is the one counter worth alarming on. +// found a well-formed frame whose CRC is wrong". Length/framing damage can be +// rejected BEFORE CRC, so zero CRC errors never proves a loss-free link. +// None of these reasons alone identifies the firmware, bridge, or host as +// the source of damage; retain raw bytes for attribution. enum class ParseReason : uint8_t { None = 0, NoHead, // first byte is not HEAD; scanned forward to the next one @@ -130,29 +138,66 @@ class FrameParser { // Bytes currently held in the resync buffer (not yet a complete frame). std::size_t buffered_bytes() const noexcept { return rx_.size(); } - // Monotonic damage counters. Everything the parser throws away lands in - // exactly one of these, so "the firmware sent fewer frames" and "we lost - // bytes on the host" stop being indistinguishable. + // Monotonic parser counters. Discarded bytes are counted as resync or + // buffer overflow; CRC and rejection reasons describe parser outcomes, + // not the physical location where data was damaged. // // Not synchronised: only the thread calling feed() may read these. The // Transport mirrors them into its own atomics from the reader thread. struct Stats { - uint64_t crc_errors = 0; // well-framed, CRC mismatch → byte loss + uint64_t crc_errors = 0; // well-framed CRC mismatch; cause unknown uint64_t resync_bytes = 0; // bytes discarded without yielding a frame uint64_t overflow_bytes = 0; // bytes dropped by the max_buffered cap + uint64_t no_head = 0; + uint64_t oversize = 0; + uint64_t short_body = 0; + uint64_t bad_length = 0; }; const Stats& stats() const noexcept { return stats_; } + // Opt-in bounded evidence, never a user callback or disk write on reader. + // Input events split large read chunks without omitting bytes. Reject + // events retain a prefix plus total available size; offsets join them to + // Input events. Time is host feed time, NOT MCU sample/exposure time. + struct TraceRecord { + enum class Kind { Input, Reject }; + Kind kind = Kind::Input; + ParseReason reason = ParseReason::None; + std::chrono::steady_clock::time_point host_time{}; + uint64_t offset = 0; + std::size_t available = 0; + std::size_t consumed = 0; + std::size_t size = 0; + std::array bytes{}; + }; + struct TraceSnapshot { + std::vector records; + uint64_t overwritten_records = 0; + }; + // Thread-safe. Enabling starts a fresh 256-record ring (~280 KiB). + // Export after stopping streaming when possible: snapshot holds a mutex. + void set_trace_enabled(bool enabled); + TraceSnapshot take_trace(); + // Clears the buffers. Damage counters are monotonic and survive. void reset(); private: void drain_(); + void trace_(TraceRecord::Kind kind, ParseReason reason, uint64_t offset, + const uint8_t* data, std::size_t available, std::size_t consumed); + void rejected_(ParseReason reason); std::vector rx_; std::deque ready_; std::size_t max_buffered_; Stats stats_{}; + uint64_t rx_offset_ = 0; + std::atomic trace_enabled_{false}; + std::mutex trace_mu_; + std::vector trace_ring_; + std::size_t trace_next_ = 0, trace_count_ = 0; + uint64_t trace_overwritten_ = 0; }; } // namespace xense::taccap::bus diff --git a/cpp/include/taccap/bus/transport.hpp b/cpp/include/taccap/bus/transport.hpp index c1e1ede..7d18781 100644 --- a/cpp/include/taccap/bus/transport.hpp +++ b/cpp/include/taccap/bus/transport.hpp @@ -1,16 +1,18 @@ // Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 // // Asynchronous transport for TC-GU-01: -// - owns a SerialBus + FrameParser + a reader thread + a dispatcher thread +// - owns a SerialBus, parser, reader, user dispatcher and control worker // - matches CMD_NEED_ACK frames to incoming ACK frames by seq, with // host-side timeout/retry honouring the firmware's 10ms / 3-attempt spec // - dispatches DATA frames to per-Cmd subscriber callbacks // -// Threading contract (two threads, deliberately): +// Threading contract: // // reader thread read() -> parse -> { ACK: fulfil the promise inline -// DATA: push to dispatch_queue_ } +// DATA: push to bounded queues } // dispatcher thread pop -> fan out to matching subscriber callbacks +// control worker motor DATA -> private SDK controller subscriptions +// (no user/Python callbacks, no logging on this path) // // The split exists because subscriber callbacks are user code: a Python // callback must take the GIL, and CPython's default 5ms switch interval means @@ -44,6 +46,7 @@ #include #include +#include #include #include #include @@ -51,11 +54,14 @@ #include #include #include +#include +#include #include #include #include #include +namespace xense::taccap { class Motor; } namespace xense::taccap::bus { // Decoded ACK / NACK from the firmware. The wire format mirrors firmware @@ -77,6 +83,7 @@ struct AckResponse { protocol::ErrorCode error_code; // Ok on success; payload[0] on NACK std::vector data; // wire payload as-is (response data on // success, [err_code] on NACK) + std::chrono::steady_clock::time_point receive_time{}; }; // Recover the firmware's error code from a response, including the echoed-cmd @@ -115,6 +122,10 @@ enum class RetryMode : uint8_t { // The firmware caches exactly one request, so nothing else may be sent on // this transport between attempts — true for the single-threaded OTA flow. SameSeq, + // Exactly one attempt regardless of Config::max_retries. For the follower + // caller that owns its own resend policy. (The leader and follower + // exchange loops use SameSeq, so a lost reply is replayed, not lost.) + Once, }; class Transport { @@ -140,6 +151,10 @@ class Transport { // queue_dropped) — lower this when latency matters more than // surviving hiccups. Clamped to >= 1. std::size_t dispatch_queue_frames = 256; + // Private motor-control lane, independent of all user callbacks. + // Overflow latches the lane closed until transport recreation: a + // dropped fault cannot be replaced silently with a healthy sample. + std::size_t control_queue_frames = 8; }; struct Stats { @@ -153,38 +168,51 @@ class Transport { uint64_t callback_exceptions = 0; // ---- Loss accounting ------------------------------------------ - // Three distinct ways a frame can fail to reach a subscriber. Read - // together they say *where* a rate drop came from: + // These counters identify the stage that observed damage or dropped + // data, not the component that originally caused a rate drop: // - // crc_errors / resync_bytes > 0 -> bytes were lost before the - // parser. Two different causes, told apart by bytes_read: - // - bytes_read also down: the host stopped draining the tty - // long enough to overflow the kernel/USB buffers. - // Cross-check callback_max_us. - // - bytes_read at full volume, crc_errors flat, resync_bytes - // climbing in ~one-frame steps: the MCU dropped bytes out of - // the middle of a frame it was transmitting, which it does - // when host->MCU traffic overlaps its own send. The short - // payload fails the LEN check before CRC is ever reached, - // which is why crc_errors stays at zero. Nothing on the host - // is broken; stop overlapping the writes (submit on the - // status-frame doorbell, as both controllers do). - // queue_dropped > 0 -> bytes arrived fine, but the - // subscriber could not keep up and old frames were evicted. - // both zero, rate still low -> the firmware really is sending - // slower; nothing on the host side to fix. - uint64_t crc_errors = 0; // well-framed, bad CRC → byte loss + // crc_errors / resync_bytes > 0 -> framing, length or CRC validation + // rejected bytes. A short frame can fail LEN before CRC, leaving + // crc_errors at zero. Neither bytes_read nor these counters can + // distinguish firmware TX, UART/USB/driver loss, host buffering, + // or a parser defect. Capturing malformed bytes in read() proves + // they reached the host before SDK parsing; locating the cause + // still needs independent link/firmware evidence. Nearby host + // writes alone do not prove physical TX/RX interference. + // queue_dropped > 0 -> parsed DATA was evicted from a full dispatch + // queue, or discarded during shutdown. Check callback_max_us and + // the run/cleanup boundaries before attributing this to load. + // all zero, rate still low -> whole-frame loss, delayed callbacks, + // component filtering or slower firmware production remain + // possible. Compare raw frames, device time and delivered data. + uint64_t crc_errors = 0; // framing/length valid; CRC mismatch uint64_t resync_bytes = 0; // bytes discarded without a frame uint64_t parser_overflow_bytes = 0; // dropped by the parser_max_buf cap + uint64_t parser_no_head = 0; + uint64_t parser_oversize = 0; + uint64_t parser_short_body = 0; + uint64_t parser_bad_length = 0; uint64_t queue_dropped = 0; // DATA frames evicted, queue full uint64_t queue_high_water = 0; // deepest the queue has ever been uint64_t callback_max_us = 0; // slowest single fan-out, in us + uint64_t control_queue_dropped = 0; + uint64_t control_queue_high_water = 0; + uint64_t control_queue_overflows = 0; // latched; controller times out }; + // Successful local writes of the four motor target commands. Counts wire + // attempts, not MCU acceptance; retries count again. No extra traffic. + struct TargetWrites { + uint64_t count = 0; + int64_t last_write_ns = 0; // steady_clock, host only + uint64_t last_gap_us = 0, max_gap_us = 0; + }; + TargetWrites target_writes(); + // Opens the serial port, starts the reader thread. explicit Transport(const Config& cfg); - // Stops the reader, fails any pending ACKs with IoError, joins. +// Stops all workers, fails any pending ACKs with IoError, joins. ~Transport(); Transport(const Transport&) = delete; @@ -230,10 +258,44 @@ class Transport { Stats stats() const noexcept; + // ---- Follower half-duplex exchange support (SDK-internal) -------------- + // While enabled, every request holds one link-wide turn from its write + // until its ACK or timeout, and no-ACK writes take the same turn: at most + // one host frame is ever waiting for the MCU, so a request is never + // written while the MCU may still be answering the previous one. Only + // meaningful when the MCU itself pushes nothing (StreamMode::Polled). + void set_half_duplex(bool enabled) noexcept { + half_duplex_.store(enabled, std::memory_order_release); + } + bool half_duplex() const noexcept { return half_duplex_.load(std::memory_order_acquire); } + // Dispatch a DATA frame decoded from an exchange reply exactly as if it + // had been received: control lane for motor status, then subscribers. + void inject_data(Frame f); + + void enable_receive_trace(bool enabled) { parser_.set_trace_enabled(enabled); } + FrameParser::TraceSnapshot take_receive_trace() { return parser_.take_trace(); } + const Config& config() const noexcept { return cfg_; } private: + friend class ::xense::taccap::Motor; + // Only SDK-owned controller subscribers may use this lane. Never expose + // arbitrary user/Python callbacks here or on the serial reader. + SubscriptionId subscribe_control_(DataCallback cb); + void control_loop_(); + void enqueue_control_(const Frame& f); + bool unsubscribe_control_(SubscriptionId id); + struct ControlSub { + SubscriptionId id; + DataCallback cb; + std::chrono::steady_clock::time_point since; + std::mutex mutex; + std::condition_variable idle; + bool active = true; + unsigned inflight = 0; + }; struct PendingAck { + protocol::Cmd cmd{}; std::promise promise; }; struct Sub { @@ -251,7 +313,26 @@ class Transport { // the kernel tty layer's, which a non-blocking fd or a non-tty transport // would quietly take away. Held across the (blocking) write, so a submit // can queue behind a large frame; that is the cost of one link. - void write_frame_(const std::vector& wire); + void write_frame_(const std::vector& wire, protocol::Cmd cmd); + uint8_t allocate_seq_locked_(); + // Taken (only while half_duplex_) around one request's write and wait. + // Never held by the reader or dispatcher, so callbacks may still send. + // Waiting for the turn counts against the request's own timeout: a long + // command (IMU calibration, 15 s) must not stretch a 100 ms query. + struct Turn { // RAII, like std::unique_lock + detail::MonotonicTimedLock* lock = nullptr; + bool owns_lock() const noexcept { return lock != nullptr; } + void unlock() { if (lock) { lock->unlock(); lock = nullptr; } } + Turn() = default; + explicit Turn(detail::MonotonicTimedLock* l) : lock(l) {} + Turn(Turn&& o) noexcept : lock(o.lock) { o.lock = nullptr; } + Turn& operator=(Turn&&) = delete; + ~Turn() { unlock(); } + }; + Turn half_duplex_turn_(std::chrono::milliseconds wait, + protocol::Cmd cmd); + detail::MonotonicTimedLock half_duplex_mu_; // not std::timed_mutex: see monotonic_cv.hpp + std::atomic half_duplex_{false}; void reader_loop_(); void dispatch_loop_(); @@ -282,9 +363,18 @@ class Transport { std::atomic running_{false}; std::thread reader_; std::thread dispatcher_; + std::thread control_worker_; + std::mutex control_queue_mu_; + std::condition_variable control_queue_cv_; + std::deque control_queue_; + bool control_failed_ = false; // control_queue_mu_ + std::mutex control_sub_mu_; + std::vector> control_subs_; + std::atomic control_sub_count_{0}; // Serialises writers; see write_frame_(). std::mutex write_mu_; + TargetWrites target_writes_; // write_mu_ // reader -> dispatcher hand-off. Bounded; drop-oldest on overflow. std::mutex queue_mu_; @@ -293,8 +383,9 @@ class Transport { // ACK matching std::mutex pending_mu_; - std::unordered_map pending_acks_; - std::atomic next_seq_{0}; + std::unordered_map> pending_acks_; + std::array reserved_seqs_{}; + uint8_t next_seq_{0}; // pending_mu_ // Subscriptions std::mutex sub_mu_; @@ -315,9 +406,16 @@ class Transport { std::atomic stat_crc_errors_ {0}; std::atomic stat_resync_bytes_ {0}; std::atomic stat_parser_overflow_ {0}; + std::atomic stat_parser_no_head_ {0}; + std::atomic stat_parser_oversize_ {0}; + std::atomic stat_parser_short_body_ {0}; + std::atomic stat_parser_bad_length_ {0}; std::atomic stat_queue_dropped_ {0}; std::atomic stat_queue_high_water_ {0}; std::atomic stat_callback_max_us_ {0}; + std::atomic stat_control_queue_dropped_{0}; + std::atomic stat_control_queue_high_water_{0}; + std::atomic stat_control_queue_overflows_{0}; }; } // namespace xense::taccap::bus diff --git a/cpp/include/taccap/components/diagnostics.hpp b/cpp/include/taccap/components/diagnostics.hpp index a692285..865f443 100644 --- a/cpp/include/taccap/components/diagnostics.hpp +++ b/cpp/include/taccap/components/diagnostics.hpp @@ -26,20 +26,10 @@ class Diagnostics { // Free-running UART counters since MCU boot (Cmd 0x54). // - // The reason this exists: when the host sees a status frame arrive short a - // couple of bytes, nothing on the host can say whether the MCU failed to - // send them or whether they were lost after leaving it. `tx_bytes_ok` and - // `tx_calls_ok` count only what the firmware's transmit call accepted, so - // comparing them against what the host decoded over the same window - // separates the two: - // - // firmware frame count == host (received + missing), bytes short - // -> lost downstream of the MCU's transmit register. Cable or - // USB-serial bridge; no firmware change reaches it. - // tx_fail_timeout > 0 - // -> the firmware truncated frames itself. - // rx_overflow > 0 - // -> the firmware's command task could not keep up with the host. + // Successful HAL-call counts are not pin-level capture. Differences from + // aligned host byte counts narrow the fault domain but cannot by themselves + // exclude MCU firmware or identify the bridge/USB/TTY layer. RX errors + // describe host-to-MCU reception, not MCU-to-host telemetry loss. // // Counters are cumulative, so take a reading before and after a // measurement window and subtract. @@ -50,6 +40,11 @@ class Diagnostics { protocol::UartStats uart_stats( std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); + // Explicit opt-in; unsupported firmware raises ProtocolError. Never + // silently report missing subtype counters as zero. No periodic polling. + protocol::UartRxErrors uart_rx_errors( + std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); + // Turn firmware logging on or off at runtime (Cmd 0x55). Returns the // configuration the firmware reports as active afterwards. // diff --git a/cpp/include/taccap/components/imu.hpp b/cpp/include/taccap/components/imu.hpp index b5a9b7d..bfa5f1b 100644 --- a/cpp/include/taccap/components/imu.hpp +++ b/cpp/include/taccap/components/imu.hpp @@ -16,6 +16,8 @@ #include #include #include +#include +#include namespace xense::taccap { @@ -24,7 +26,11 @@ struct ImuSample { // since the MCU's `timestamp_us` is relative to its own boot. std::chrono::steady_clock::time_point host_time; - uint32_t mcu_timestamp_us; // MCU-side timestamp (us since MCU boot) + uint64_t mcu_timestamp_us; // V2: exact capture time in the boot epoch. + uint64_t boot_session_id{}; // V2: nonzero, constant per boot. Legacy: 0. + bool timestamp_is_64bit{}; // True only for native V2, not legacy unfolding. + bool timestamp_valid{true}; // False on ambiguous legacy discontinuities. + bool timestamp_discontinuity{}; // New boot / ambiguous jump; reset estimators. uint16_t valid_flag; // protocol::ImuValid::* bits uint16_t seq; // MCU-side packet sequence @@ -46,15 +52,42 @@ class IMU { explicit IMU(bus::Transport& transport); - // Synchronous one-shot read (sends CMD_NEED_ACK GetImu, waits for ACK). - // Throws on timeout / NACK / size mismatch. + // Negotiate time format, then GetImu/GetImuV2. Timeout is per command + // (plus transport retries). Throws on timeout / NACK / size mismatch. ImuSample read_once(std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); + // Read-only capability negotiation except for assigning a first-boot RAM + // nonce. Repeated calls preserve the existing nonce. No flash writes. + uint16_t negotiate_time(std::chrono::milliseconds timeout = std::chrono::milliseconds{500}); + + // One tracker per subscription (and one for polling), so fan-out does not + // consume another subscriber's reset notification. Legacy epochs are + // relative to first observation, not guaranteed absolute boot uptime. + class Timeline { + public: + ImuSample decode(const std::uint8_t*, std::size_t, + std::chrono::steady_clock::time_point arrival = std::chrono::steady_clock::now()); + private: + bool seen_{}; + // Legacy cannot establish a new boot epoch after an ambiguous jump. + // Keep it invalid until a new tracker (subscription/instance) or V2. + bool legacy_faulted_{}; + uint64_t last_time_{}, session_{}; + uint32_t raw_{}; + uint16_t seq_{}; + std::chrono::steady_clock::time_point host_{}; + }; // Subscribe to streaming DATA frames. The Transport keeps streaming // running independently — see LeaderGripper::start_streaming(). SubId on_data(Callback cb); void off(SubId id); + // Keep the IMU stationary, sensor Z up. Caller must stop the controller + // and disable the motor first on followers. Cmd 0x25 actions 1 then 4: + // firmware performs its static verification before we request persistence. + // timeout applies to each command; timeout does NOT cancel device work. + void calibrate_accel_gyro(std::chrono::milliseconds timeout = std::chrono::milliseconds{16000}); + // V1.4 — write magnetometer hard-iron offsets + soft-iron correction // matrix to firmware (Cmd::SetImuMagCal, 0x26). `hard_iron` is the // 3-axis bias in µT; `soft_iron` is the 3×3 correction matrix in @@ -64,8 +97,10 @@ class IMU { const std::array& soft_iron_row_major, std::chrono::milliseconds timeout = std::chrono::milliseconds{500}); - // Magnetometer calibration session (Cmd 0x26 with a 1-byte payload; leader - // firmware only -- a follower NACKs InvalidCmd). + // Magnetometer calibration session (Cmd 0x26 with a 1-byte payload). + // Historical followers NACK; capable followers require stopped/disabled + // motors, restore the previous stream on finish, and expire after 180s. + // The following stream-replacement semantics apply to leader firmware: // // start: the firmware flags calibration and STARTS AN IMU STREAM ON ITS OWN // (separate mode, IMU only, rate code 5) so the host can collect raw @@ -86,6 +121,9 @@ class IMU { private: bus::Transport& t_; + std::mutex read_mutex_, subscriptions_mutex_; + Timeline polling_time_; + std::unordered_map subscriptions_; }; } // namespace xense::taccap diff --git a/cpp/include/taccap/components/motor.hpp b/cpp/include/taccap/components/motor.hpp index 02032ad..0def7b9 100644 --- a/cpp/include/taccap/components/motor.hpp +++ b/cpp/include/taccap/components/motor.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include namespace xense::taccap { @@ -156,6 +158,11 @@ namespace detail { bool is_stray_fault_reply_frame(bool seen_real_temp, float motor_temp_c) noexcept; } // namespace detail +struct MotorDeliverySnapshot { + bus::Transport::TargetWrites before, after; + protocol::MotorControlStats firmware{}; +}; + class Motor { public: using SubId = bus::Transport::SubscriptionId; @@ -192,34 +199,23 @@ class Motor { // NO NACK, NO retry, NO timeout, and NO throw on a target the firmware // rejects. The firmware's slave control task consumes the *latest* // submitted target. Unlike set_*(), which block on an ACK and throw - // ProtocolError on NACK, submit() never blocks. + // ProtocolError on NACK, submit() does not wait for an ACK; its serial + // write can still block or throw an I/O error. // // ---- On submission rate ------------------------------------------------ // This used to say you could submit "up to the firmware's 500Hz slave // control rate", which read as a budget to spend. It is not one. // - // The 500Hz figure is real — that is how often the firmware's control task - // applies the latest target — but it says nothing about what submitting at - // that rate costs you. Every host->MCU frame that lands while the MCU is - // transmitting makes it drop bytes out of the middle of the frame it is - // sending, and that frame is then discarded whole (tc-gu-01 issue #1). A - // 41-byte status frame at 3 Mbps fills only ~137us of each 10ms period, so - // whether a given submit collides is down to *when* it lands, not how many - // you send: - // - // - 250Hz lost 154 status frames on one 60s run and none on the next. - // - 300Hz was clean on a run where 250Hz was not. - // - 1000Hz has produced both 0 and 146 lost frames on the same firmware. - // - // So rate does not predict loss; it only sets how many chances to collide - // you take per second. Submitting faster than the status stream also buys - // nothing on the observation side, since motor status is capped at 100Hz. - // - // Prefer ImpedanceController or ForcePositionController, which submit once - // per received status frame and therefore never overlap a transmission — - // stream-locked submission measured at zero lost frames across 8 runs while - // sending MORE frames than the free-running comparison. Reach for raw submit() at your own cadence - // only when you cannot ride the status stream. + // The MCU control task applies the latest accepted target at 500 Hz. + // This is not a guarantee of host throughput or loss-free delivery. + // Historical concurrent-traffic tests found intermittent short telemetry + // frames; the rate alone did not predict them or establish the failing + // layer. Full-duplex overlap is not itself evidence of a firmware defect. + // + // The controllers pace updates from status feedback. That does not prove + // the UART is idle: IMU or ACK frames may immediately follow status. + // target_writes() observes host cadence without I/O; delivery_snapshot() + // explicitly queries MCU acceptance off the control thread. // // Separately: sustained input above a few hundred Hz used to livelock the // firmware's command handler outright (tc-gu-01 issue #2, fixed in firmware @@ -493,6 +489,15 @@ class Motor { // boot and needs no parameter access at all. protocol::MotorPrivateParam get_private_param(uint16_t index); void set_private_param(uint16_t index, uint32_t raw_value); + // One explicit 0x51 query, bracketed by local target-write counters. + // Compare two snapshots on the same connection/boot. The range captures + // query overlap, NOT arbitrary in-flight buffering. Counter differences + // indicate a health concern, not exact packet loss. No automatic retries + // of control targets, logging, background queries or stop policy changes. + MotorDeliverySnapshot delivery_snapshot( + std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); + bus::Transport::TargetWrites target_writes() { return t_.target_writes(); } + protocol::MotorControlStats control_stats( // Cmd 0x51 std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); @@ -522,7 +527,20 @@ class Motor { static MotorStatusSample decode(const std::uint8_t* payload, std::size_t len); private: + friend class ForcePositionController; + friend class ImpedanceController; + friend class FollowerGripper; + // SDK-owned bounded control callbacks only; never user code. + SubId on_control_status_(Callback cb); + SubId subscribe_status_(Callback cb, bool control); + // While FollowerGripper runs the half-duplex exchange, submit() hands the + // encoded target to it instead of writing a no-ACK frame. Empty = direct. + using TargetRouter = std::function)>; + void set_target_router_(TargetRouter router); + void submit_(protocol::Cmd cmd, std::vector payload); bus::Transport& t_; + std::mutex router_mu_; + std::shared_ptr router_; }; } // namespace xense::taccap diff --git a/cpp/include/taccap/detail/monotonic_cv.hpp b/cpp/include/taccap/detail/monotonic_cv.hpp new file mode 100644 index 0000000..c5cb59a --- /dev/null +++ b/cpp/include/taccap/detail/monotonic_cv.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// Timed waits that really run on CLOCK_MONOTONIC. +// +// Why: libstdc++ only implements condition_variable::wait_until(steady_clock) +// and timed_mutex::try_lock_for on the monotonic clock when it was built +// against glibc >= 2.30 (pthread_cond_clockwait / pthread_mutex_clocklock). +// Toolchains built on an older sysroot -- conda-forge's, manylinux's -- fall +// back to pthread_cond_timedwait / pthread_mutex_timedlock on CLOCK_REALTIME: +// a steady deadline is converted to wall-clock time, so when NTP steps the +// wall clock back by X, the wait lasts X longer. That stalled a half-duplex +// exchange loop past the controllers' 200 ms status timeout (observed: +// "Clock change detected" 0.2 s before the fault). +// +// A pthread condition variable whose attribute selects CLOCK_MONOTONIC is +// available in every glibc, so these waits are immune to wall-clock steps +// whatever the toolchain. std::chrono::steady_clock is CLOCK_MONOTONIC on +// Linux (libstdc++ and libc++), so its time points convert directly. + +#pragma once + +#include +#include + +#include +#include + +namespace xense::taccap::detail { + +class MonotonicCondition { +public: + MonotonicCondition() { + pthread_condattr_t attr; + pthread_condattr_init(&attr); + pthread_condattr_setclock(&attr, CLOCK_MONOTONIC); + pthread_cond_init(&cond_, &attr); + pthread_condattr_destroy(&attr); + } + ~MonotonicCondition() { pthread_cond_destroy(&cond_); } + MonotonicCondition(const MonotonicCondition&) = delete; + MonotonicCondition& operator=(const MonotonicCondition&) = delete; + + void notify_one() noexcept { pthread_cond_signal(&cond_); } + void notify_all() noexcept { pthread_cond_broadcast(&cond_); } + + // Same contract as std::condition_variable::wait_until with a predicate: + // returns pred() (false only when the deadline passed with pred false). + template + bool wait_until(std::unique_lock& lk, + std::chrono::steady_clock::time_point deadline, Pred pred) { + while (!pred()) { + if (std::chrono::steady_clock::now() >= deadline) return pred(); + const auto ns = std::chrono::duration_cast( + deadline.time_since_epoch()).count(); + timespec ts{}; + ts.tv_sec = static_cast(ns / 1'000'000'000); + ts.tv_nsec = static_cast(ns % 1'000'000'000); + pthread_cond_timedwait(&cond_, lk.mutex()->native_handle(), &ts); + } + return true; + } + + template + bool wait_for(std::unique_lock& lk, + std::chrono::duration d, Pred pred) { + return wait_until(lk, std::chrono::steady_clock::now() + + std::chrono::duration_cast(d), + std::move(pred)); + } + +private: + pthread_cond_t cond_; +}; + +// A lock that can be waited for with a monotonic timeout (replacement for +// std::timed_mutex, whose try_lock_for has the same wall-clock fallback). +class MonotonicTimedLock { +public: + bool try_lock_for(std::chrono::steady_clock::duration wait) { + std::unique_lock lk(mu_); + if (!cv_.wait_for(lk, wait, [this] { return !held_; })) return false; + held_ = true; + return true; + } + void unlock() { + { + std::lock_guard lk(mu_); + held_ = false; + } + cv_.notify_one(); + } + +private: + std::mutex mu_; + MonotonicCondition cv_; + bool held_ = false; +}; + +} // namespace xense::taccap::detail diff --git a/cpp/include/taccap/error.hpp b/cpp/include/taccap/error.hpp index da25065..015dc7d 100644 --- a/cpp/include/taccap/error.hpp +++ b/cpp/include/taccap/error.hpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace xense::taccap { @@ -28,6 +29,17 @@ class CrcError : public ProtocolError { using ProtocolError::ProtocolError; }; +// Preserve ProtocolError compatibility while allowing capability negotiation +// to fall back ONLY on an explicit unsupported-command response. +class NackError : public ProtocolError { +public: + NackError(const std::string& what, std::uint8_t code) + : ProtocolError(what), code_(code) {} + std::uint8_t code() const noexcept { return code_; } +private: + std::uint8_t code_; +}; + // Errno-backed I/O failure (open/read/write/tcsetattr). class IoError : public Error { public: diff --git a/cpp/include/taccap/follower_gripper.hpp b/cpp/include/taccap/follower_gripper.hpp index 1a1bbf6..d0edb5b 100644 --- a/cpp/include/taccap/follower_gripper.hpp +++ b/cpp/include/taccap/follower_gripper.hpp @@ -214,8 +214,25 @@ std::optional expected_open_reverse(std::uint8_t model_id, std::optional expected_motor_can_id(std::uint8_t model_id, discovery::Side side) noexcept; +class FollowerExchange; + } // namespace detail +// Counters of FollowerGripper's half-duplex exchange since start_exchange(). +// timeouts are exchanges without a reply; a target in one is resent in the +// next exchange unless superseded (targets_resent). targets_rejected counts +// replies whose target_result was not Ok; last_target_result is the raw +// ErrorCode byte of the latest target reply (0xFF before any). +struct ExchangeStats { + uint64_t exchanges = 0, telemetry_exchanges = 0, target_exchanges = 0; + uint64_t timeouts = 0, nacks = 0, malformed = 0; + uint64_t targets_submitted = 0, targets_superseded = 0, targets_resent = 0; + uint64_t targets_rejected = 0; + uint8_t last_target_result = 0xFF; + uint64_t motor_samples = 0, imu_samples = 0, imu_dropped = 0; + uint64_t last_rtt_us = 0, max_rtt_us = 0; +}; + class FollowerGripper { public: struct Config { @@ -478,7 +495,7 @@ class FollowerGripper { // // Motor status is the ONLY thing a follower streams. This used to mirror // the leader's (imu_hz, encoder_hz, motor_hz) signature, which was worse - // than useless: the follower firmware emits IMU, encoder and eskin only + // than useless: historical follower firmware emitted IMU/encoder/eskin only // under #ifdef ENABLE_MASTER_GRIPPER, so those two rates set their mask // bits and then produced nothing (measured: 1000Hz of each yields zero // frames and leaves byte volume unchanged). Worse, the old defaults were @@ -501,9 +518,35 @@ class FollowerGripper { // for a bad rate, so the SDK logs a warning whenever it has to adjust one. // See cpp/src/stream_rate.hpp for the firmware model this mirrors. void start_streaming(unsigned motor_hz = 100); + // Opt-in IMU on capable follower firmware, separate 28-byte packets. + // imu_hz=0 preserves legacy behavior; 1..100 enables IMU after a GetImu + // capability probe. Old firmware NACKs without changing its stream. + // motor_hz=0 permits IMU-only; both zero is invalid. Stop before changing + // an existing stream (as with the original one-argument interface). + void start_streaming(unsigned motor_hz, unsigned imu_hz); void stop_streaming(); bool is_streaming() const noexcept { return streaming_; } + // Half-duplex alternative to start_streaming() (firmware with 0x5D). + // + // The follower is put in StreamMode::Polled: it samples as configured but + // pushes nothing. A host thread sends one exchange per 1/exchange_hz s and + // receives motor status and new IMU samples in the reply; Motor::submit*() + // targets ride on the next exchange (sent at once if the link is idle). + // All requests on this transport, including user queries, take turns, so + // the host never writes while the follower may be answering. This removes + // the full-duplex overlap that loses uplink bytes on the current hardware. + // + // Subscribers (motor().on_status, imu().on_data) and the controllers see + // the same frames as with start_streaming(); is_streaming() is true and + // stop_streaming() ends the exchange. exchange_hz=0 means max(motor_hz, + // imu_hz, 1). Firmware without the polled mode NACKs: ProtocolError, and + // nothing is left running. + void start_exchange(unsigned motor_hz = 100, unsigned imu_hz = 0, + unsigned exchange_hz = 0); + bool is_exchanging() const noexcept { return exchange_ != nullptr; } + ExchangeStats exchange_stats() const; + const Config& config() const noexcept { return cfg_; } private: @@ -541,6 +584,9 @@ class FollowerGripper { OtaSession ota_; // V1.3 std::unique_ptr wrist_; bool streaming_ = false; + uint16_t streaming_sources_ = 0; + std::shared_ptr exchange_; + void stop_exchange_() noexcept; GripperPosition pos_map_; // raw<->position, cached bool pos_map_loaded_ = false; }; diff --git a/cpp/include/taccap/force_position_controller.hpp b/cpp/include/taccap/force_position_controller.hpp index 426067d..3ff9697 100644 --- a/cpp/include/taccap/force_position_controller.hpp +++ b/cpp/include/taccap/force_position_controller.hpp @@ -58,6 +58,7 @@ #pragma once +#include #include #include #include @@ -430,7 +431,7 @@ class ForcePositionController { GripperPosition map_; mutable std::mutex mu_; - std::condition_variable cv_; + detail::MonotonicCondition cv_; // status watchdog: must not stretch on a wall-clock step bool step_requested_ = false; bool stop_requested_ = false; bool have_sample_ = false; @@ -458,6 +459,9 @@ class ForcePositionController { std::vector queue_; void enqueue_(const QueuedCommand& c); MotorStatusSample latest_{}; + // Lightweight first-fault latch, consumed/logged only by run_(). + const char* pending_status_fault_ = nullptr; + std::chrono::steady_clock::time_point feedback_epoch_{}; std::chrono::steady_clock::time_point latest_time_{}; GripperObservation observation_{}; std::unique_ptr policy_; diff --git a/cpp/include/taccap/impedance_controller.hpp b/cpp/include/taccap/impedance_controller.hpp index a95b26d..a275b15 100644 --- a/cpp/include/taccap/impedance_controller.hpp +++ b/cpp/include/taccap/impedance_controller.hpp @@ -96,6 +96,7 @@ #pragma once +#include #include #include #include @@ -311,13 +312,15 @@ class ImpedanceController { GripperPosition map_; mutable std::mutex mu_; - std::condition_variable cv_; + detail::MonotonicCondition cv_; // status watchdog: must not stretch on a wall-clock step bool step_requested_ = false; bool stop_requested_ = false; bool have_sample_ = false; bool command_woke_ = false; MotorStatusSample latest_{}; + const char* pending_status_fault_ = nullptr; + std::chrono::steady_clock::time_point feedback_epoch_{}; std::chrono::steady_clock::time_point latest_time_{}; GripperObservation observation_{}; std::unique_ptr policy_; diff --git a/cpp/include/taccap/leader_gripper.hpp b/cpp/include/taccap/leader_gripper.hpp index 4dd91c2..2caf68b 100644 --- a/cpp/include/taccap/leader_gripper.hpp +++ b/cpp/include/taccap/leader_gripper.hpp @@ -48,6 +48,29 @@ namespace xense::taccap { +namespace detail { +class LeaderExchange; +} // namespace detail + +// Counters of LeaderGripper's half-duplex exchange since start_exchange(). +// records counts every record received; the per-kind counters split them. +// records_dropped is what the firmware queue overflowed before it was +// fetched. timeouts are exchanges without a reply even after same-seq retries. +// continuation_exchanges went out immediately because the firmware reported a +// backlog. not_polled counts replies saying the firmware is not in the +// exchange: its 1 s lease expired while this host stalled, another process +// restarted the stream, or the leader rebooted. Each such reply makes the SDK +// re-send the polled StartStream (rearms; rearm_failures when that failed). +struct LeaderExchangeStats { + uint64_t exchanges = 0, continuation_exchanges = 0; + uint64_t timeouts = 0, nacks = 0, malformed = 0, not_polled = 0; + uint64_t rearms = 0, rearm_failures = 0; + uint64_t records = 0, records_dropped = 0; + uint64_t imu_samples = 0, encoder_samples = 0, key_events = 0; + uint64_t sensor_errors = 0, other_records = 0; + uint64_t last_rtt_us = 0, max_rtt_us = 0; +}; + class LeaderGripper { public: struct Config { @@ -204,10 +227,61 @@ class LeaderGripper { // anything above 1000 Hz collapses to 100 Hz. StartStream is never NACKed // for a bad rate, so the SDK logs a warning whenever it has to adjust one. // See cpp/src/stream_rate.hpp for the firmware model this mirrors. - void start_streaming(unsigned imu_hz = 100, unsigned encoder_hz = 100); + // + // imu_v2_time (default true): negotiate the 64-bit time session and stream + // GetImuV2 (44 B payload) when the firmware supports it; false always + // streams the legacy GetImu (28 B) without a time session, as SDK <= 0.4.4 + // did. The pushed stream loses an uplink frame whenever a host write + // overlaps it; a longer frame is hit more often (measured on a leader at + // 100 Hz queries: IMU bad frames ~4.7x with V2). If the stream must stay + // pushed and queries are frequent, pass false; start_exchange() removes the + // overlap altogether and keeps V2. + void start_streaming(unsigned imu_hz = 100, unsigned encoder_hz = 100, + bool imu_v2_time = true); void stop_streaming(); bool is_streaming() const noexcept { return streaming_; } + // Half-duplex alternative to start_streaming() (firmware with 0x5E). + // + // The leader is put in StreamMode::Polled: it samples as configured but + // pushes nothing -- IMU, encoder, key events and sensor errors are held + // in the firmware. A host thread fetches them once per 1/exchange_hz s + // (immediately again while a backlog remains) and dispatches them as the + // DATA frames streaming would have delivered, so imu().on_data, + // encoder().on_data, key().on_event and sensor-error subscribers need no + // change. All requests on this transport take turns, so the host never + // writes while the leader may be answering; this removes the full-duplex + // overlap that loses uplink bytes on the current hardware. + // + // is_streaming() is true and stop_streaming() ends the exchange. + // exchange_hz=0 means max(imu_hz, encoder_hz, 1) capped at 200 (the leader + // samples at 100 Hz whatever is requested); an explicit exchange_hz may be up + // to 500. A sample is + // fetched on the next exchange after it is taken, so it waits at most one + // exchange period; at exchange_hz == data rate the two clocks drift, and a + // sample occasionally waits one extra period (~10 ms at 100 Hz, measured + // ~330 times per hour). Pass exchange_hz = 2x the data rate (e.g. 200) to + // remove that, at the cost of twice the (small) request traffic. + // + // Timestamps: every sample delivered from one exchange reply carries that + // reply's arrival time as host_time, i.e. up to one exchange period after + // the sample was taken. Use mcu_timestamp_us for the sampling instant. + // + // The firmware leaves the exchange by itself if no exchange arrives for + // 1 s (host crashed or stalled): it flushes what it held as ordinary DATA + // frames and goes back to pushing, so a dead host never leaves the leader + // silent. If this process was only stalled, the next reply says so and the + // SDK re-enters the exchange; nothing is lost in between. + // + // Firmware without 0x5E answers InvalidCmd. With fallback_to_streaming + // (the default) the SDK then calls start_streaming(imu_hz, encoder_hz) + // and returns false; otherwise it throws ProtocolError. Returns true when + // the exchange is running. + bool start_exchange(unsigned imu_hz = 100, unsigned encoder_hz = 100, + unsigned exchange_hz = 0, bool fallback_to_streaming = true); + bool is_exchanging() const noexcept { return exchange_ != nullptr; } + LeaderExchangeStats exchange_stats() const; + const Config& config() const noexcept { return cfg_; } private: @@ -244,6 +318,8 @@ class LeaderGripper { OtaSession ota_; // V1.3 std::unique_ptr wrist_; bool streaming_ = false; + std::shared_ptr exchange_; + void stop_exchange_() noexcept; GripperPosition pos_map_; // raw<->position, cached bool pos_map_loaded_ = false; }; diff --git a/cpp/include/taccap/protocol/codec.hpp b/cpp/include/taccap/protocol/codec.hpp index 13ac1fe..d596647 100644 --- a/cpp/include/taccap/protocol/codec.hpp +++ b/cpp/include/taccap/protocol/codec.hpp @@ -128,6 +128,7 @@ GripperAutoCalConfig decode_gripper_auto_cal_config(const uint8_t* data, std::si MotorControlStats decode_motor_control_stats(const uint8_t* data, std::size_t len); // fw 1.1.3 shipped a 32-byte packet; 1.1.4 appended log_dropped. Both decode // here, with the missing tail zero-filled, so one SDK build talks to either. +UartRxErrors decode_uart_rx_errors(const uint8_t* data, std::size_t len); UartStats decode_uart_stats(const uint8_t* data, std::size_t len); LogConfig decode_log_config(const uint8_t* data, std::size_t len); StreamConfig decode_stream_config(const uint8_t* data, std::size_t len); diff --git a/cpp/include/taccap/protocol/commands.hpp b/cpp/include/taccap/protocol/commands.hpp index 2e78bb7..e62af65 100644 --- a/cpp/include/taccap/protocol/commands.hpp +++ b/cpp/include/taccap/protocol/commands.hpp @@ -76,6 +76,9 @@ enum class Cmd : uint8_t { GetEskin2 = 0x13, GetAllSensors = 0x14, KeyStatus = 0x15, // V1.4 — device-side button, payload=key_status_payload_t + TimeSession = 0x16, // request nonce:u64; response time_us:u64, session:u64 + GetImuV2 = 0x17, // legacy 28B + capture_us:u64 + session:u64 + TxTrace = 0x18, // RAM transmit trace; diagnostic firmware builds only // Stream control + calibration (0x20–0x2F) StartStream = 0x20, @@ -136,6 +139,13 @@ enum class Cmd : uint8_t { MotorCanExtXfer = 0x5B, // extended-frame passthrough (req 24B / resp 16B) SetMotorFwVersion = 0x5C, // store motor fw version in flash (req 4B / resp 8B), // fw >= 1.2.14 + // Follower half-duplex exchange: request may carry one 0x40..0x43 target, + // reply carries motor status V2 + new IMU samples. Used with + // StreamMode::Polled; see FollowerGripper::start_exchange(). + SlaveExchange = 0x5D, + // Leader half-duplex exchange: reply carries the DATA frames the leader + // held in StreamMode::Polled. See LeaderGripper::start_exchange(). + LeaderExchange = 0x5E, // Diagnostics, firmware 1.1.3+ / 1.1.4+. Present on leader and follower // alike: the counters live in the firmware's UART layer, not in a // gripper-role-specific subsystem. diff --git a/cpp/include/taccap/protocol/payloads.hpp b/cpp/include/taccap/protocol/payloads.hpp index c5c9ce6..66198ae 100644 --- a/cpp/include/taccap/protocol/payloads.hpp +++ b/cpp/include/taccap/protocol/payloads.hpp @@ -755,13 +755,8 @@ static_assert(sizeof(GripperEnvelope) == 16, // ---- UART counters (fw 1.1.3 — Cmd::GetUartStats 0x54) -------------------- // -// Exists to answer "which side dropped the bytes". tx_bytes_ok counts only -// what HAL_UART_Transmit accepted, i.e. what actually reached the MCU's -// transmit register. When that disagrees with what the host received, the loss -// happened downstream of the MCU — cable or USB-serial bridge — and no -// firmware change can help. tx_fail_timeout is called out separately because a -// polled transmit that times out mid-frame leaves a short frame on the wire, -// which looks identical to bytes lost in transit. +// Counts successful HAL calls, not pin-level observations. Aligned byte +// differences do not alone distinguish MCU, bridge, USB and TTY faults. // // All counters are free-running since boot; take deltas across a measurement // window rather than reading absolutes. @@ -783,6 +778,13 @@ struct UartStats { // silenced the command channel for good. }; +// Optional 0x54 payload {1}: 44-byte legacy prefix + seven uint32 LE fields. +struct UartRxErrors { + uint32_t overrun, framing, noise, parity, receiver_timeout; + uint32_t last_error, last_error_ms; +}; +static_assert(sizeof(UartRxErrors) == 28); + // ---- Log configuration (fw 1.1.4 — Cmd::SetLogConfig 0x55) ---------------- // // Firmware logging is OFF by default and this is the only way to turn it back @@ -983,6 +985,7 @@ struct Ws2812Effect { namespace StreamSrc { constexpr uint16_t Imu = 0x0001; + constexpr uint16_t ImuV2 = 0x0020; constexpr uint16_t Encoder = 0x0002; constexpr uint16_t Eskin1 = 0x0004; constexpr uint16_t Eskin2 = 0x0008; @@ -992,6 +995,64 @@ namespace StreamSrc { enum class StreamMode : uint8_t { Separate = 0, Combined = 1, + // Acquisition runs as configured but nothing is pushed; the host pulls it + // with Cmd::SlaveExchange (follower) or Cmd::LeaderExchange (leader). + // Older firmware NACKs it. + Polled = 2, +}; + +// ---- Leader half-duplex exchange (Cmd::LeaderExchange, 0x5E) -------------- +// Request: LeaderExchangeRequest, exact length, flags/reserved zero. +// Reply: LeaderExchangeResponseHeader, then record_count records, oldest first: +// [cmd:1][len:1][payload:len] +// Each record is the DATA frame the leader would have pushed outside the +// exchange (same cmd, same payload): IMU, encoder, key, sensor error. +namespace LeaderExchangeRespFlag { constexpr uint8_t Polled = 0x01; } +constexpr uint8_t LEADER_EXCHANGE_VERSION = 1; + +struct LeaderExchangeRequest { + uint8_t version; // LEADER_EXCHANGE_VERSION + uint8_t flags; // 0 + uint8_t reserved[2]; +}; + +struct LeaderExchangeResponseHeader { + uint8_t version; // LEADER_EXCHANGE_VERSION + uint8_t flags; // LeaderExchangeRespFlag + uint8_t record_count; + uint8_t reserved; + uint16_t dropped; // records the firmware queue overflowed since the previous take + uint16_t pending; // records still queued after this reply +}; + +// ---- Follower half-duplex exchange (Cmd::SlaveExchange, 0x5D) ------------- +// Request: ExchangeRequestHeader + the target struct for target_kind, exact +// length. Reply: ExchangeResponseHeader, then MotorStatus V2 (59 B) when +// ExchangeRespFlag::Motor is set, then imu_count IMU packets whose format is +// imu_cmd (GetImu 28 B or GetImuV2 44 B), oldest first. +namespace ExchangeFlag { constexpr uint8_t Telemetry = 0x01; } +namespace ExchangeRespFlag { constexpr uint8_t Motor = 0x01; } +enum class ExchangeTarget : uint8_t { + None = 0, Position = 1, Velocity = 2, Torque = 3, Impedance = 4, +}; +constexpr uint8_t EXCHANGE_VERSION = 1; +constexpr uint8_t EXCHANGE_TARGET_RESULT_NONE = 0xFF; + +struct ExchangeRequestHeader { + uint8_t version; // EXCHANGE_VERSION + uint8_t flags; // ExchangeFlag + uint8_t target_kind; // ExchangeTarget + uint8_t reserved; +}; + +struct ExchangeResponseHeader { + uint8_t version; // EXCHANGE_VERSION + uint8_t flags; // ExchangeRespFlag + uint8_t target_result; // ErrorCode of the carried target, 0xFF if none + uint8_t imu_cmd; // 0, Cmd::GetImu or Cmd::GetImuV2 + uint8_t imu_count; + uint8_t imu_dropped; // firmware queue overflow since the previous take + uint8_t reserved[2]; }; enum class StreamInterface : uint8_t { @@ -1159,6 +1220,10 @@ struct OtaStatus { static_assert(sizeof(FirmwareVersion) == 4); static_assert(sizeof(SnInfo) == 17); static_assert(sizeof(ImuData) == 28); // IMU_PACKET_SIZE +static_assert(sizeof(ExchangeRequestHeader) == 4); +static_assert(sizeof(ExchangeResponseHeader) == 8); +static_assert(sizeof(LeaderExchangeRequest) == 4); +static_assert(sizeof(LeaderExchangeResponseHeader) == 8); static_assert(sizeof(ImuConfig) == 10); static_assert(sizeof(EncoderData) == 16); // ENCODER_PACKET_SIZE static_assert(sizeof(EncoderConfig) == 5); // firmware 0086da6 (was 14) diff --git a/cpp/src/bus/frame.cpp b/cpp/src/bus/frame.cpp index 51d8306..8a58cb2 100644 --- a/cpp/src/bus/frame.cpp +++ b/cpp/src/bus/frame.cpp @@ -188,6 +188,13 @@ FrameParser::FrameParser(std::size_t max_buffered) : max_buffered_(max_buffered) void FrameParser::feed(const uint8_t* data, std::size_t len) { if (data && len) { + if (trace_enabled_.load(std::memory_order_relaxed)) { + for (std::size_t i = 0; i < len; i += 1024) { + const auto n = std::min(1024, len - i); + trace_(TraceRecord::Kind::Input, ParseReason::None, + rx_offset_ + rx_.size() + i, data + i, n, 0); + } + } rx_.insert(rx_.end(), data, data + len); } drain_(); @@ -205,10 +212,64 @@ bool FrameParser::try_pop(Frame& out) { } void FrameParser::reset() { + rx_offset_ += rx_.size(); rx_.clear(); ready_.clear(); } +void FrameParser::set_trace_enabled(bool enabled) { + std::lock_guard lock(trace_mu_); + trace_enabled_.store(false, std::memory_order_relaxed); + trace_ring_.clear(); + trace_next_ = trace_count_ = 0; + trace_overwritten_ = 0; + if (enabled) trace_ring_.resize(256); + trace_enabled_.store(enabled, std::memory_order_relaxed); +} + +FrameParser::TraceSnapshot FrameParser::take_trace() { + std::lock_guard lock(trace_mu_); + TraceSnapshot result; + result.overwritten_records = trace_overwritten_; + result.records.reserve(trace_count_); + const auto start = trace_count_ == trace_ring_.size() ? trace_next_ : 0; + for (std::size_t i = 0; i < trace_count_; ++i) + result.records.push_back(trace_ring_[(start + i) % trace_ring_.size()]); + trace_count_ = trace_next_ = 0; + return result; +} + +void FrameParser::trace_(TraceRecord::Kind kind, ParseReason reason, + uint64_t offset, const uint8_t* data, + std::size_t available, std::size_t consumed) { + if (!trace_enabled_.load(std::memory_order_relaxed)) return; + std::lock_guard lock(trace_mu_); + if (!trace_enabled_.load(std::memory_order_relaxed)) return; + auto& r = trace_ring_[trace_next_]; + r.kind = kind; + r.reason = reason; + r.host_time = std::chrono::steady_clock::now(); + r.offset = offset; + r.available = available; + r.consumed = consumed; + r.size = std::min(available, r.bytes.size()); + std::copy_n(data, r.size, r.bytes.begin()); + trace_next_ = (trace_next_ + 1) % trace_ring_.size(); + if (trace_count_ == trace_ring_.size()) ++trace_overwritten_; + else ++trace_count_; +} + +void FrameParser::rejected_(ParseReason reason) { + switch (reason) { + case ParseReason::NoHead: ++stats_.no_head; break; + case ParseReason::Oversize: ++stats_.oversize; break; + case ParseReason::ShortBody: ++stats_.short_body; break; + case ParseReason::BadLength: ++stats_.bad_length; break; + case ParseReason::BadCrc: ++stats_.crc_errors; break; + case ParseReason::None: break; + } +} + void FrameParser::drain_() { std::size_t cursor = 0; @@ -221,8 +282,10 @@ void FrameParser::drain_() { continue; } if (r.status == ParseStatus::Resync) { - if (r.reason == ParseReason::BadCrc) ++stats_.crc_errors; + rejected_(r.reason); const std::size_t step = (r.consumed > 0 ? r.consumed : 1); + trace_(TraceRecord::Kind::Reject, r.reason, rx_offset_ + cursor, + rx_.data() + cursor, rx_.size() - cursor, step); stats_.resync_bytes += step; cursor += step; continue; @@ -247,12 +310,16 @@ void FrameParser::drain_() { // Counted once here rather than per speculative step, so the // scan below can't double-bill the same bytes. stats_.resync_bytes += (alt - cursor); + trace_(TraceRecord::Kind::Reject, ParseReason::NoHead, + rx_offset_ + cursor, rx_.data() + cursor, alt - cursor, + alt - cursor); cursor = alt + r2.consumed; recovered = true; break; } if (r2.status == ParseStatus::Resync) { - if (r2.reason == ParseReason::BadCrc) ++stats_.crc_errors; + // Speculative lookahead has not discarded anything. Do not + // count it repeatedly when subsequent reads revisit it. alt += (r2.consumed > 0 ? r2.consumed : 1); continue; } @@ -267,6 +334,7 @@ void FrameParser::drain_() { // Drop everything we consumed from the front of the buffer. We use a // single erase rather than per-byte pops to keep this O(n) overall. if (cursor > 0) { + rx_offset_ += cursor; rx_.erase(rx_.begin(), rx_.begin() + static_cast(cursor)); } @@ -274,6 +342,9 @@ void FrameParser::drain_() { if (rx_.size() > max_buffered_) { const std::size_t excess = rx_.size() - max_buffered_; stats_.overflow_bytes += excess; + trace_(TraceRecord::Kind::Reject, ParseReason::Oversize, rx_offset_, + rx_.data(), excess, excess); + rx_offset_ += excess; rx_.erase(rx_.begin(), rx_.begin() + static_cast(excess)); } } diff --git a/cpp/src/bus/serial_bus.cpp b/cpp/src/bus/serial_bus.cpp index 38912f0..81f35d0 100644 --- a/cpp/src/bus/serial_bus.cpp +++ b/cpp/src/bus/serial_bus.cpp @@ -146,7 +146,7 @@ void SerialBus::apply_termios_() { tio.c_cflag &= ~(CSIZE | PARENB | CSTOPB | CRTSCTS); tio.c_cflag |= CS8 | CREAD | CLOCAL; - tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | + tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INPCK | IGNPAR | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY); tio.c_oflag &= ~(OPOST); tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); diff --git a/cpp/src/bus/transport.cpp b/cpp/src/bus/transport.cpp index afaadba..62a2350 100644 --- a/cpp/src/bus/transport.cpp +++ b/cpp/src/bus/transport.cpp @@ -20,10 +20,12 @@ Transport::Transport(const Config& cfg) serial_(cfg.serial), parser_(cfg.parser_max_buf) { if (cfg_.dispatch_queue_frames == 0) cfg_.dispatch_queue_frames = 1; + if (cfg_.control_queue_frames == 0) cfg_.control_queue_frames = 1; running_.store(true, std::memory_order_release); // Dispatcher first: the reader must never find itself without a consumer. dispatcher_ = std::thread(&Transport::dispatch_loop_, this); try { + control_worker_ = std::thread(&Transport::control_loop_, this); reader_ = std::thread(&Transport::reader_loop_, this); } catch (...) { // The ctor did not complete, so ~Transport never runs — and a joinable @@ -60,84 +62,138 @@ AckResponse Transport::send_cmd(protocol::Cmd cmd, const std::vector& payload, std::chrono::milliseconds timeout, RetryMode retry) { - if (!running_.load(std::memory_order_acquire)) { - throw IoError("send_cmd on stopped transport", EBADF); - } const auto t = (timeout.count() == 0) ? cfg_.ack_timeout : timeout; - - uint8_t seq = 0; - bool seq_allocated = false; - - for (unsigned attempt = 0; attempt <= cfg_.max_retries; ++attempt) { - // SameSeq keeps the first attempt's seq for every retry so the - // firmware recognises the repeat and replays its cached response - // rather than running a non-idempotent handler twice. Allocating - // lazily keeps NewSeq's wire behaviour byte-for-byte unchanged. - if (!seq_allocated || retry == RetryMode::NewSeq) { - seq = next_seq_.fetch_add(1, std::memory_order_relaxed); - seq_allocated = true; - } - std::future fut; - { - std::lock_guard lk(pending_mu_); - // unique_ptr-style emplace: default-construct the PendingAck. - auto& p = pending_acks_[seq]; - // If the slot was somehow taken (uint8 wrap collision with a - // very-late stray ACK), reset its promise. - p.promise = std::promise{}; - fut = p.promise.get_future(); - } - - try { + uint8_t seq = 0; + bool reserved = false; + std::shared_ptr pending; + auto release = [&]() { + std::lock_guard lk(pending_mu_); + if (!reserved) return; + auto it = pending_acks_.find(seq); + if (it != pending_acks_.end() && it->second == pending) + pending_acks_.erase(it); + reserved_seqs_[seq] = false; + reserved = false; + }; + const unsigned max_retries = retry == RetryMode::Once ? 0u : cfg_.max_retries; + try { + for (unsigned attempt = 0; attempt <= max_retries; ++attempt) { + if (reserved && retry == RetryMode::NewSeq) release(); + std::future fut; + { + std::lock_guard lk(pending_mu_); + if (!running_.load(std::memory_order_acquire)) + throw IoError("send_cmd on stopped transport", EBADF); + if (!reserved) { + seq = allocate_seq_locked_(); + reserved_seqs_[seq] = true; + reserved = true; + } + pending = std::make_shared(); + pending->cmd = cmd; + fut = pending->promise.get_future(); + pending_acks_[seq] = pending; + } + const auto attempt_start = std::chrono::steady_clock::now(); + auto turn = half_duplex_turn_(t, cmd); write_frame_(pack_frame(cfg_.peer, seq, - protocol::FrameType::CMD_NEED_ACK, - cmd, payload)); - } catch (...) { - std::lock_guard lk(pending_mu_); - pending_acks_.erase(seq); - throw; - } - - const auto status = fut.wait_for(t); - if (status == std::future_status::ready) { - // Note: pending entry already erased by handle_ack_ on fulfilment. - return fut.get(); // may rethrow IoError set by fail_pending_ - } - - // Timeout — clean up entry, retry. - { - std::lock_guard lk(pending_mu_); - pending_acks_.erase(seq); - } - stat_ack_timeouts_.fetch_add(1, std::memory_order_relaxed); - if (attempt < cfg_.max_retries) { - stat_retries_.fetch_add(1, std::memory_order_relaxed); - std::this_thread::sleep_for(cfg_.retry_interval); + protocol::FrameType::CMD_NEED_ACK, cmd, payload), cmd); + const auto left = t - std::chrono::duration_cast( + std::chrono::steady_clock::now() - attempt_start); + const bool answered = + fut.wait_for(std::max(left, std::chrono::milliseconds(1))) == std::future_status::ready; + if (turn.owns_lock()) turn.unlock(); + if (answered) { + auto ack = fut.get(); + release(); + return ack; + } + { + std::lock_guard lk(pending_mu_); + auto it = pending_acks_.find(seq); + if (it != pending_acks_.end() && it->second == pending) + pending_acks_.erase(it); + } + // SameSeq owns its slot throughout retry delays (OTA deduplication). + stat_ack_timeouts_.fetch_add(1, std::memory_order_relaxed); + if (attempt < max_retries) { + stat_retries_.fetch_add(1, std::memory_order_relaxed); + std::this_thread::sleep_for(cfg_.retry_interval); + } } + throw TimeoutError("send_cmd " + std::string(protocol::to_string(cmd)) + + ": no ACK after " + std::to_string(max_retries + 1) + + " attempts", ETIMEDOUT); + } catch (...) { + release(); + throw; } +} - throw TimeoutError("send_cmd " + std::string(protocol::to_string(cmd)) + - ": no ACK after " + - std::to_string(cfg_.max_retries + 1) + " attempts", - ETIMEDOUT); +// pending_mu_ guards reservation across the entire request lifetime, including +// timeout/ACK races and SameSeq retries. Never overwrite another promise. +uint8_t Transport::allocate_seq_locked_() { + for (unsigned i = 0; i < 256; ++i) { + const uint8_t seq = next_seq_++; + if (!reserved_seqs_[seq]) return seq; + } + throw IoError("Transport: all ACK sequence slots occupied", EBUSY); } void Transport::send_cmd_no_ack(protocol::Cmd cmd, const std::vector& payload) { - if (!running_.load(std::memory_order_acquire)) { - throw IoError("send_cmd_no_ack on stopped transport", EBADF); + uint8_t seq; + { + std::lock_guard lk(pending_mu_); + if (!running_.load(std::memory_order_acquire)) + throw IoError("send_cmd_no_ack on stopped transport", EBADF); + seq = allocate_seq_locked_(); } - const uint8_t seq = next_seq_.fetch_add(1, std::memory_order_relaxed); + auto turn = half_duplex_turn_(cfg_.ack_timeout, cmd); write_frame_(pack_frame(cfg_.peer, seq, - protocol::FrameType::CMD_NO_ACK, cmd, payload)); + protocol::FrameType::CMD_NO_ACK, cmd, payload), cmd); +} + +Transport::Turn Transport::half_duplex_turn_(std::chrono::milliseconds wait, protocol::Cmd cmd) { + if (!half_duplex_.load(std::memory_order_acquire)) return {}; + if (!half_duplex_mu_.try_lock_for(wait)) { + throw TimeoutError("send " + std::string(protocol::to_string(cmd)) + + ": half-duplex link busy for " + std::to_string(wait.count()) + " ms", + ETIMEDOUT); + } + return Turn(&half_duplex_mu_); +} + +void Transport::inject_data(Frame f) { + f.type = protocol::FrameType::DATA; + if (f.cmd == protocol::Cmd::GetMotorStatus) enqueue_control_(f); + enqueue_data_(std::move(f)); +} + +Transport::TargetWrites Transport::target_writes() { + std::lock_guard lk(write_mu_); + return target_writes_; } // The one place bytes leave for the MCU. Packing stays outside the lock; only // the write is serialised. -void Transport::write_frame_(const std::vector& wire) { +void Transport::write_frame_(const std::vector& wire, protocol::Cmd cmd) { { std::lock_guard lk(write_mu_); serial_.write(wire); + if (cmd == protocol::Cmd::MotorPosCtrl || cmd == protocol::Cmd::MotorVelCtrl || + cmd == protocol::Cmd::MotorTorqueCtrl || cmd == protocol::Cmd::MotorImpedanceCtrl) { + const auto now = std::chrono::steady_clock::now(); + const auto ns = std::chrono::duration_cast( + now.time_since_epoch()).count(); + if (target_writes_.count != 0) { + target_writes_.last_gap_us = (ns - target_writes_.last_write_ns) / 1000; + target_writes_.max_gap_us = std::max(target_writes_.max_gap_us, + target_writes_.last_gap_us); + } + ++target_writes_.count; + target_writes_.last_write_ns = ns; + } } stat_bytes_written_.fetch_add(wire.size(), std::memory_order_relaxed); stat_frames_sent_.fetch_add(1, std::memory_order_relaxed); @@ -152,6 +208,7 @@ Transport::subscribe(protocol::Cmd cmd, DataCallback cb) { } void Transport::unsubscribe(SubscriptionId id) { + if (unsubscribe_control_(id)) return; std::lock_guard lk(sub_mu_); subs_.erase(std::remove_if(subs_.begin(), subs_.end(), [id](const Sub& s) { return s.id == id; }), @@ -163,6 +220,8 @@ bool Transport::is_running() const noexcept { } void Transport::clear_subs_() noexcept { + { std::lock_guard lock(control_sub_mu_); + control_subs_.clear(); control_sub_count_.store(0); } std::vector doomed; { std::lock_guard lk(sub_mu_); @@ -182,6 +241,10 @@ void Transport::join_workers_() noexcept { // within one VTIME tick regardless of how slow subscribers are. if (reader_.joinable()) reader_.join(); + { std::lock_guard lk(control_queue_mu_); } + control_queue_cv_.notify_all(); + if (control_worker_.joinable()) control_worker_.join(); + // Taking queue_mu_ here closes the lost-wakeup window: if the dispatcher // has evaluated its predicate but not yet blocked, it still holds the // mutex, so this blocks until it is genuinely waiting on the cv. @@ -225,6 +288,11 @@ void Transport::stop() noexcept { } fail_pending_("transport stopped"); + { + std::lock_guard lock(control_queue_mu_); + stat_control_queue_dropped_.fetch_add(control_queue_.size()); + control_queue_.clear(); + } } Transport::Stats Transport::stats() const noexcept { @@ -240,9 +308,16 @@ Transport::Stats Transport::stats() const noexcept { s.crc_errors = stat_crc_errors_.load(std::memory_order_relaxed); s.resync_bytes = stat_resync_bytes_.load(std::memory_order_relaxed); s.parser_overflow_bytes = stat_parser_overflow_.load(std::memory_order_relaxed); + s.parser_no_head = stat_parser_no_head_.load(std::memory_order_relaxed); + s.parser_oversize = stat_parser_oversize_.load(std::memory_order_relaxed); + s.parser_short_body = stat_parser_short_body_.load(std::memory_order_relaxed); + s.parser_bad_length = stat_parser_bad_length_.load(std::memory_order_relaxed); s.queue_dropped = stat_queue_dropped_.load(std::memory_order_relaxed); s.queue_high_water = stat_queue_high_water_.load(std::memory_order_relaxed); s.callback_max_us = stat_callback_max_us_.load(std::memory_order_relaxed); + s.control_queue_dropped = stat_control_queue_dropped_.load(std::memory_order_relaxed); + s.control_queue_high_water = stat_control_queue_high_water_.load(std::memory_order_relaxed); + s.control_queue_overflows = stat_control_queue_overflows_.load(std::memory_order_relaxed); return s; } @@ -261,6 +336,7 @@ void Transport::reader_loop_() { return; } if (n == 0) continue; // VTIME wakeup, no data + const auto received = std::chrono::steady_clock::now(); stat_bytes_read_.fetch_add(n, std::memory_order_relaxed); parser_.feed(buf.data(), n); @@ -272,9 +348,14 @@ void Transport::reader_loop_() { stat_crc_errors_.store(ps.crc_errors, std::memory_order_relaxed); stat_resync_bytes_.store(ps.resync_bytes, std::memory_order_relaxed); stat_parser_overflow_.store(ps.overflow_bytes, std::memory_order_relaxed); + stat_parser_no_head_.store(ps.no_head, std::memory_order_relaxed); + stat_parser_oversize_.store(ps.oversize, std::memory_order_relaxed); + stat_parser_short_body_.store(ps.short_body, std::memory_order_relaxed); + stat_parser_bad_length_.store(ps.bad_length, std::memory_order_relaxed); Frame f; while (parser_.try_pop(f)) { + f.receive_time = received; stat_frames_received_.fetch_add(1, std::memory_order_relaxed); dispatch_(std::move(f)); } @@ -312,6 +393,7 @@ void Transport::dispatch_(Frame&& f) { handle_ack_(f); break; case protocol::FrameType::DATA: + if (f.cmd == protocol::Cmd::GetMotorStatus) enqueue_control_(f); enqueue_data_(std::move(f)); break; case protocol::FrameType::CMD_NEED_ACK: @@ -330,8 +412,10 @@ void Transport::handle_ack_(const Frame& f) { { std::lock_guard lk(pending_mu_); auto it = pending_acks_.find(f.seq); - if (it != pending_acks_.end()) { - winning_promise = std::move(it->second.promise); + if (it != pending_acks_.end() && + (f.cmd == it->second->cmd || + (static_cast(f.cmd) == 0 && f.payload.size() == 1))) { + winning_promise = std::move(it->second->promise); pending_acks_.erase(it); found = true; } @@ -347,6 +431,7 @@ void Transport::handle_ack_(const Frame& f) { r.cmd = f.cmd; r.is_nack = false; r.data = f.payload; + r.receive_time = f.receive_time; if (static_cast(f.cmd) == 0) { // cmd == 0 means the firmware took the protocol_send_ack(seq, err) @@ -364,8 +449,8 @@ void Transport::handle_ack_(const Frame& f) { r.error_code = err; try { winning_promise.set_exception(std::make_exception_ptr( - ProtocolError(std::string("NACK: ") + - protocol::to_string(err)))); + NackError(std::string("NACK: ") + + protocol::to_string(err), static_cast(err)))); } catch (...) { /* promise already satisfied — ignore */ } return; } @@ -442,11 +527,104 @@ void Transport::handle_data_(const Frame& f) { } } +Transport::SubscriptionId Transport::subscribe_control_(DataCallback cb) { + { + std::lock_guard lock(control_queue_mu_); + if (control_failed_) throw IoError("motor control receive lane overflowed; reopen transport", EOVERFLOW); + } + auto sub = std::make_shared(); + sub->id = next_sub_id_.fetch_add(1, std::memory_order_relaxed); + sub->cb = std::move(cb); + sub->since = std::chrono::steady_clock::now(); + std::lock_guard lock(control_sub_mu_); + control_subs_.push_back(sub); + control_sub_count_.fetch_add(1, std::memory_order_release); + return sub->id; +} + +bool Transport::unsubscribe_control_(SubscriptionId id) { + std::shared_ptr sub; + { + std::lock_guard lock(control_sub_mu_); + auto it = std::find_if(control_subs_.begin(), control_subs_.end(), + [id](const auto& s) { return s->id == id; }); + if (it == control_subs_.end()) return false; + sub = *it; + control_subs_.erase(it); + control_sub_count_.fetch_sub(1, std::memory_order_release); + } + std::unique_lock lock(sub->mutex); + sub->active = false; + // A copied subscription may already be executing. Controller destruction + // must wait for it; simply removing the callback from a vector is unsafe. + sub->idle.wait(lock, [&] { return sub->inflight == 0; }); + return true; +} + +void Transport::enqueue_control_(const Frame& f) { + if (!control_sub_count_.load(std::memory_order_acquire)) return; + { + std::lock_guard lock(control_queue_mu_); + if (control_failed_) { + stat_control_queue_dropped_.fetch_add(1, std::memory_order_relaxed); + return; + } + if (control_queue_.size() >= cfg_.control_queue_frames) { + // Do not silently evict an old fault and continue on a healthy + // newest sample. Stop delivering control feedback; its existing + // receive-age watchdog must then fail safe, even if user delivery + // continues. Recreating the transport explicitly clears the latch. + control_failed_ = true; + stat_control_queue_overflows_.fetch_add(1, std::memory_order_relaxed); + stat_control_queue_dropped_.fetch_add(control_queue_.size() + 1, + std::memory_order_relaxed); + control_queue_.clear(); + return; + } + control_queue_.push_back(f); + const auto depth = control_queue_.size(); + auto high = stat_control_queue_high_water_.load(std::memory_order_relaxed); + if (depth > high) stat_control_queue_high_water_.store(depth, std::memory_order_relaxed); + } + control_queue_cv_.notify_one(); +} + +void Transport::control_loop_() { + for (;;) { + Frame frame; + { + std::unique_lock lock(control_queue_mu_); + control_queue_cv_.wait(lock, [&] { + return stop_requested_.load(std::memory_order_acquire) || !control_queue_.empty(); + }); + if (stop_requested_.load(std::memory_order_acquire)) return; + frame = std::move(control_queue_.front()); + control_queue_.pop_front(); + } + std::vector> hits; + { std::lock_guard lock(control_sub_mu_); hits = control_subs_; } + for (const auto& sub : hits) { + { + std::lock_guard lock(sub->mutex); + if (!sub->active || frame.receive_time < sub->since) continue; + ++sub->inflight; + } + try { sub->cb(frame); } + catch (...) { stat_callback_exceptions_.fetch_add(1, std::memory_order_relaxed); } + { + std::lock_guard lock(sub->mutex); + --sub->inflight; + } + sub->idle.notify_all(); + } + } +} + void Transport::fail_pending_(const std::string& reason) noexcept { std::lock_guard lk(pending_mu_); for (auto& kv : pending_acks_) { try { - kv.second.promise.set_exception(std::make_exception_ptr( + kv.second->promise.set_exception(std::make_exception_ptr( IoError("transport: " + reason, EIO))); } catch (...) { /* already satisfied */ } } diff --git a/cpp/src/components/diagnostics.cpp b/cpp/src/components/diagnostics.cpp index 0488bf0..bb2f217 100644 --- a/cpp/src/components/diagnostics.cpp +++ b/cpp/src/components/diagnostics.cpp @@ -8,6 +8,15 @@ namespace xense::taccap { Diagnostics::Diagnostics(bus::Transport& transport) : t_(transport) {} +protocol::UartRxErrors Diagnostics::uart_rx_errors(std::chrono::milliseconds timeout) { + auto ack = t_.send_cmd(protocol::Cmd::GetUartStats, {1}, timeout); + const auto error = bus::ack_error_code(ack); + if (error != protocol::ErrorCode::Ok) + throw ProtocolError(std::string("Diagnostics::uart_rx_errors NACK: ") + + protocol::to_string(error)); + return protocol::decode_uart_rx_errors(ack.data.data(), ack.data.size()); +} + protocol::UartStats Diagnostics::uart_stats(std::chrono::milliseconds timeout) { auto ack = t_.send_cmd(protocol::Cmd::GetUartStats, {}, timeout); if (ack.is_nack) { diff --git a/cpp/src/components/encoder.cpp b/cpp/src/components/encoder.cpp index 47cff26..9bc8c1f 100644 --- a/cpp/src/components/encoder.cpp +++ b/cpp/src/components/encoder.cpp @@ -49,6 +49,7 @@ EncoderSample Encoder::read_once(std::chrono::milliseconds timeout) { protocol::to_string(ack.error_code)); } auto s = decode(ack.data.data(), ack.data.size()); + s.host_time = ack.receive_time; normalize(s); return s; } @@ -135,6 +136,7 @@ Encoder::SubId Encoder::on_data(Callback cb) { [this, cb = std::move(cb)](const bus::Frame& f) { try { auto s = decode(f.payload.data(), f.payload.size()); + s.host_time = f.receive_time; normalize(s); cb(s); } catch (...) {} diff --git a/cpp/src/components/imu.cpp b/cpp/src/components/imu.cpp index 8e2c838..f788824 100644 --- a/cpp/src/components/imu.cpp +++ b/cpp/src/components/imu.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include namespace xense::taccap { @@ -22,6 +24,12 @@ constexpr float kCdpsToRps = 0.01f * static_cast(M_PI) / 180.0f; constexpr float kCuTtoUT = 0.01f; constexpr float kCdegToDeg = 0.01f; +uint64_t read64(const uint8_t* p) { + uint64_t v = 0; + for (unsigned i = 0; i < 8; ++i) v |= uint64_t(p[i]) << (8 * i); + return v; +} + } // namespace IMU::IMU(bus::Transport& transport) : t_(transport) {} @@ -29,9 +37,16 @@ IMU::IMU(bus::Transport& transport) : t_(transport) {} ImuSample IMU::decode(const std::uint8_t* payload, std::size_t len) { ImuSample s{}; s.host_time = std::chrono::steady_clock::now(); - s.raw = protocol::decode_imu(payload, len); // throws on size mismatch + s.raw = protocol::decode_imu(payload, len == 44 ? 28 : len); s.mcu_timestamp_us = s.raw.timestamp_us; + if (len == 44) { + s.mcu_timestamp_us = read64(payload + 28); + s.boot_session_id = read64(payload + 36); + s.timestamp_is_64bit = true; + if (!s.boot_session_id || uint32_t(s.mcu_timestamp_us) != s.raw.timestamp_us) + throw ProtocolError("IMU V2: invalid session or inconsistent timestamp"); + } s.valid_flag = s.raw.valid_flag; s.seq = s.raw.seq; @@ -54,30 +69,131 @@ ImuSample IMU::decode(const std::uint8_t* payload, std::size_t len) { return s; } +ImuSample IMU::Timeline::decode(const uint8_t* p, std::size_t n, + std::chrono::steady_clock::time_point arrival) { + auto s = IMU::decode(p, n); + s.host_time = arrival; + if (seen_) { + const auto gap = std::chrono::duration_cast(s.host_time - host_).count(); + if (s.timestamp_is_64bit) { + s.timestamp_discontinuity = session_ != s.boot_session_id; + if (!s.timestamp_discontinuity && s.mcu_timestamp_us < last_time_) { + s.timestamp_valid = false; + s.timestamp_discontinuity = true; + return s; // stale frame: do not move the high-water mark + } + legacy_faulted_ = false; + } else { + const uint32_t delta = s.raw.timestamp_us - raw_; + const uint16_t seq_delta = uint16_t(s.seq - seq_); + // Legacy cannot prove boot identity or reconstruct arbitrary gaps. + // Never turn a historical 17-second DWT reset into a 71-minute wrap. + bool ambiguous = legacy_faulted_ || session_ != 0 || gap < 0 || gap >= INT32_MAX || + delta >= INT32_MAX || seq_delta >= 0x8000U; + if (s.raw.timestamp_us < raw_) + ambiguous |= raw_ < 0xf0000000U || s.raw.timestamp_us > 0x0fffffffU || + seq_delta == 0 || uint64_t(delta) > uint64_t(gap) + 2000000ULL; + s.timestamp_valid = !ambiguous; + s.timestamp_discontinuity = ambiguous; + legacy_faulted_ = ambiguous; + if (!ambiguous) s.mcu_timestamp_us = last_time_ + delta; + } + } + seen_ = true; + last_time_ = s.mcu_timestamp_us; + raw_ = s.raw.timestamp_us; + seq_ = s.seq; + session_ = s.boot_session_id; + host_ = s.host_time; + return s; +} + +uint16_t IMU::negotiate_time(std::chrono::milliseconds timeout) { + std::random_device random; + uint64_t nonce = (uint64_t(random()) << 32) ^ uint64_t(random()); + if (!nonce) nonce = 1; + std::vector request(8); + for (unsigned i = 0; i < 8; ++i) request[i] = uint8_t(nonce >> (8 * i)); + bus::AckResponse ack; + try { + ack = t_.send_cmd(protocol::Cmd::TimeSession, request, timeout); + } catch (const NackError& e) { + if (e.code() == uint8_t(protocol::ErrorCode::InvalidCmd)) return protocol::StreamSrc::Imu; + throw; + } + const auto error = bus::ack_error_code(ack); + if (error == protocol::ErrorCode::InvalidCmd) return protocol::StreamSrc::Imu; + if (error != protocol::ErrorCode::Ok || ack.data.size() != 16 || !read64(ack.data.data() + 8)) + throw ProtocolError("Invalid IMU time-session response"); + return protocol::StreamSrc::ImuV2; +} + ImuSample IMU::read_once(std::chrono::milliseconds timeout) { - auto ack = t_.send_cmd(protocol::Cmd::GetImu, {}, timeout); + std::lock_guard lock(read_mutex_); + const auto cmd = negotiate_time(timeout) == protocol::StreamSrc::ImuV2 + ? protocol::Cmd::GetImuV2 : protocol::Cmd::GetImu; + auto ack = t_.send_cmd(cmd, {}, timeout); if (ack.is_nack) { throw ProtocolError(std::string("IMU::read_once NACK: ") + protocol::to_string(ack.error_code)); } - return decode(ack.data.data(), ack.data.size()); + if (ack.data.size() != (cmd == protocol::Cmd::GetImuV2 ? 44U : 28U)) + throw ProtocolError("IMU response does not match negotiated format"); + return polling_time_.decode(ack.data.data(), ack.data.size(), ack.receive_time); } IMU::SubId IMU::on_data(Callback cb) { - return t_.subscribe( - protocol::Cmd::GetImu, - [cb = std::move(cb)](const bus::Frame& f) { + struct SubscriptionState { + Timeline timeline; + std::vector last_payload; + }; + auto state = std::make_shared(); + auto callback = [cb = std::move(cb), state](const bus::Frame& f) { // Dispatcher thread context, not the reader -- Transport keeps // user code off the read path on purpose. decode() may throw on a - // malformed frame; swallow here (Transport already counts - // callback_exc). + // malformed frame; preserve the component's existing policy of + // dropping malformed frames and containing callback exceptions. try { - cb(decode(f.payload.data(), f.payload.size())); + if (f.payload.size() != (f.cmd == protocol::Cmd::GetImuV2 ? 44U : 28U)) + throw ProtocolError("IMU stream format mismatch"); + // Legacy devices may send the identical cache at 1000 Hz + // although acquisition is 100 Hz. Do not integrate it twice. + if (f.payload == state->last_payload) return; + auto sample = state->timeline.decode(f.payload.data(), f.payload.size(), f.receive_time); + state->last_payload = f.payload; + cb(sample); } catch (...) {} - }); + }; + std::lock_guard lock(subscriptions_mutex_); + auto legacy = t_.subscribe(protocol::Cmd::GetImu, callback); + try { + auto v2 = t_.subscribe(protocol::Cmd::GetImuV2, callback); + try { subscriptions_.emplace(legacy, v2); } + catch (...) { t_.unsubscribe(v2); throw; } + } catch (...) { t_.unsubscribe(legacy); throw; } + return legacy; } -void IMU::off(SubId id) { t_.unsubscribe(id); } +void IMU::off(SubId id) { + std::lock_guard lock(subscriptions_mutex_); + auto it = subscriptions_.find(id); + if (it != subscriptions_.end()) { + t_.unsubscribe(it->second); + subscriptions_.erase(it); + } + t_.unsubscribe(id); +} + +void IMU::calibrate_accel_gyro(std::chrono::milliseconds timeout) { + for (uint8_t action : {0x01, 0x04}) { + auto ack = t_.send_cmd(protocol::Cmd::SetImuCal, {action}, timeout, + bus::RetryMode::SameSeq); + if (ack.is_nack) { + throw ProtocolError(std::string("IMU::calibrate_accel_gyro NACK: ") + + protocol::to_string(ack.error_code)); + } + } +} void IMU::start_mag_calibration(std::chrono::milliseconds timeout) { auto ack = t_.send_cmd(protocol::Cmd::SetImuMagCal, {0x01}, timeout); diff --git a/cpp/src/components/motor.cpp b/cpp/src/components/motor.cpp index 2b9bbfa..f2f4d4a 100644 --- a/cpp/src/components/motor.cpp +++ b/cpp/src/components/motor.cpp @@ -95,16 +95,33 @@ void Motor::set_impedance(float pos, float kp, float kd, float ff_torque, // throw on NACK (there is none). The struct overloads are the single source of // truth; the float wrappers delegate. See motor.hpp for the health contract. void Motor::submit(const protocol::MotorImpedanceCtrl& c) { - t_.send_cmd_no_ack(protocol::Cmd::MotorImpedanceCtrl, protocol::encode(c)); + submit_(protocol::Cmd::MotorImpedanceCtrl, protocol::encode(c)); } void Motor::submit(const protocol::MotorPosCtrl& c) { - t_.send_cmd_no_ack(protocol::Cmd::MotorPosCtrl, protocol::encode(c)); + submit_(protocol::Cmd::MotorPosCtrl, protocol::encode(c)); } void Motor::submit(const protocol::MotorVelCtrl& c) { - t_.send_cmd_no_ack(protocol::Cmd::MotorVelCtrl, protocol::encode(c)); + submit_(protocol::Cmd::MotorVelCtrl, protocol::encode(c)); } void Motor::submit(const protocol::MotorTorqueCtrl& c) { - t_.send_cmd_no_ack(protocol::Cmd::MotorTorqueCtrl, protocol::encode(c)); + submit_(protocol::Cmd::MotorTorqueCtrl, protocol::encode(c)); +} + +void Motor::set_target_router_(TargetRouter router) { + std::shared_ptr next; + if (router) next = std::make_shared(std::move(router)); + std::lock_guard lk(router_mu_); + router_ = std::move(next); +} + +void Motor::submit_(protocol::Cmd cmd, std::vector payload) { + std::shared_ptr router; + { + std::lock_guard lk(router_mu_); + router = router_; + } + if (router) (*router)(cmd, std::move(payload)); + else t_.send_cmd_no_ack(cmd, payload); } void Motor::submit_impedance(float pos, float kp, float kd, float ff_torque, @@ -127,7 +144,9 @@ MotorStatusSample Motor::read_status(std::chrono::milliseconds timeout) { throw ProtocolError(std::string("Motor::read_status NACK: ") + protocol::to_string(ack.error_code)); } - return decode(ack.data.data(), ack.data.size()); + auto sample = decode(ack.data.data(), ack.data.size()); + sample.host_time = ack.receive_time; + return sample; } protocol::MotorStatusExt Motor::read_status_ext(std::chrono::milliseconds timeout) { @@ -310,19 +329,26 @@ bool is_stray_fault_reply_frame(bool seen_real_temp, float motor_temp_c) noexcep } // namespace detail Motor::SubId Motor::on_status(Callback cb) { + return subscribe_status_(std::move(cb), false); +} + +Motor::SubId Motor::on_control_status_(Callback cb) { + return subscribe_status_(std::move(cb), true); +} + +Motor::SubId Motor::subscribe_status_(Callback cb, bool control) { // Per subscription: the dispatch thread is the only caller, so no lock. auto seen_real_temp = std::make_shared(false); auto dropped = std::make_shared(0); - return t_.subscribe( - protocol::Cmd::GetMotorStatus, - [cb = std::move(cb), seen_real_temp, dropped](const bus::Frame& f) { + auto callback = [cb = std::move(cb), seen_real_temp, dropped, control](const bus::Frame& f) { try { - const MotorStatusSample s = decode(f.payload.data(), f.payload.size()); + MotorStatusSample s = decode(f.payload.data(), f.payload.size()); + s.host_time = f.receive_time; if (detail::is_stray_fault_reply_frame(*seen_real_temp, s.motor_temp_c)) { // Rate-limited: a closing move on an affected unit draws a few // per second. Powers of two keep the count visible. const std::uint64_t n = ++*dropped; - if ((n & (n - 1)) == 0) { + if (!control && (n & (n - 1)) == 0) { logger()->warn( "Motor::on_status: dropped {} status frame(s) that were not " "status (temperature 0.0, pos {:.4f} rad, torque {:.2f} N*m): " @@ -336,7 +362,9 @@ Motor::SubId Motor::on_status(Callback cb) { if (s.motor_temp_c != 0.0f) *seen_real_temp = true; cb(s); } catch (...) {} - }); + }; + return control ? t_.subscribe_control_(std::move(callback)) + : t_.subscribe(protocol::Cmd::GetMotorStatus, std::move(callback)); } void Motor::off(SubId id) { t_.unsubscribe(id); } @@ -422,6 +450,14 @@ void Motor::set_private_param(uint16_t index, uint32_t raw_value) { send_or_throw(t_, protocol::Cmd::MotorSetPrivateParam, req, "set_private_param"); } +MotorDeliverySnapshot Motor::delivery_snapshot(std::chrono::milliseconds timeout) { + MotorDeliverySnapshot out; + out.before = t_.target_writes(); + out.firmware = control_stats(timeout); + out.after = t_.target_writes(); + return out; +} + protocol::MotorControlStats Motor::control_stats(std::chrono::milliseconds timeout) { auto ack = t_.send_cmd(protocol::Cmd::GetMotorControlStats, {}, timeout); if (ack.is_nack) { diff --git a/cpp/src/follower_exchange.cpp b/cpp/src/follower_exchange.cpp new file mode 100644 index 0000000..ed55119 --- /dev/null +++ b/cpp/src/follower_exchange.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 + +#include "follower_exchange.hpp" + +#include + +#include + +namespace xense::taccap::detail { + +namespace { + +using clock = std::chrono::steady_clock; + +std::optional target_kind(protocol::Cmd cmd) { + switch (cmd) { + case protocol::Cmd::MotorPosCtrl: return protocol::ExchangeTarget::Position; + case protocol::Cmd::MotorVelCtrl: return protocol::ExchangeTarget::Velocity; + case protocol::Cmd::MotorTorqueCtrl: return protocol::ExchangeTarget::Torque; + case protocol::Cmd::MotorImpedanceCtrl: return protocol::ExchangeTarget::Impedance; + default: return std::nullopt; + } +} + +std::size_t imu_size(uint8_t cmd) { + if (cmd == static_cast(protocol::Cmd::GetImu)) return sizeof(protocol::ImuData); + if (cmd == static_cast(protocol::Cmd::GetImuV2)) return sizeof(protocol::ImuData) + 16; + return 0; +} + +} // namespace + +FollowerExchange::FollowerExchange(bus::Transport& transport, + std::chrono::microseconds period, + std::chrono::milliseconds timeout) + : t_(transport), period_(period), timeout_(timeout) { + worker_ = std::thread([this] { run_(); }); +} + +FollowerExchange::~FollowerExchange() { stop(); } + +void FollowerExchange::stop() noexcept { + { + std::lock_guard lk(mu_); + stop_ = true; + } + cv_.notify_all(); + if (worker_.joinable() && worker_.get_id() != std::this_thread::get_id()) worker_.join(); +} + +void FollowerExchange::submit(protocol::Cmd cmd, std::vector payload) { + const auto kind = target_kind(cmd); + if (!kind) throw IoError("FollowerExchange: not a motor target command", EINVAL); + { + std::lock_guard lk(mu_); + if (stop_) return; + if (pending_) ++stats_.targets_superseded; + pending_ = Target{*kind, std::move(payload)}; + ++stats_.targets_submitted; + } + cv_.notify_all(); +} + +ExchangeStats FollowerExchange::stats() const { + std::lock_guard lk(mu_); + return stats_; +} + +void FollowerExchange::run_() { + auto next_tick = clock::now(); + std::unique_lock lk(mu_); + while (!stop_) { + cv_.wait_until(lk, next_tick, [&] { return stop_ || pending_.has_value(); }); + if (stop_) break; + const bool telemetry = clock::now() >= next_tick; + std::optional target = std::move(pending_); + pending_.reset(); + lk.unlock(); + + protocol::ExchangeRequestHeader head{}; + head.version = protocol::EXCHANGE_VERSION; + head.flags = telemetry ? protocol::ExchangeFlag::Telemetry : 0; + head.target_kind = static_cast(target ? target->kind : protocol::ExchangeTarget::None); + std::vector req(sizeof(head)); + std::memcpy(req.data(), &head, sizeof(head)); + if (target) req.insert(req.end(), target->payload.begin(), target->payload.end()); + + const auto started = clock::now(); + std::optional ack; + bool link_down = false; + try { + // SameSeq: when a reply is lost the retry is byte-identical, so the + // firmware's command layer replays its cached reply (same telemetry, + // the target not applied twice) instead of running a new exchange -- + // otherwise the IMU samples that reply carried would be lost. + ack = t_.send_cmd(protocol::Cmd::SlaveExchange, req, timeout_, bus::RetryMode::SameSeq); + } catch (const TimeoutError&) { + } catch (const std::exception&) { + link_down = !t_.is_running(); + } + const auto rtt_us = static_cast( + std::chrono::duration_cast(clock::now() - started).count()); + + if (ack) handle_reply_(*ack, target.has_value()); + + lk.lock(); + ++stats_.exchanges; + if (telemetry) ++stats_.telemetry_exchanges; + if (target) ++stats_.target_exchanges; + if (ack) { + stats_.last_rtt_us = rtt_us; + if (rtt_us > stats_.max_rtt_us) stats_.max_rtt_us = rtt_us; + } else { + ++stats_.timeouts; + // Not acknowledged: resend unless a newer target replaced it. + if (target && !pending_) { + pending_ = std::move(target); + ++stats_.targets_resent; + } + } + if (link_down) break; + if (telemetry) { + next_tick += period_; + const auto now = clock::now(); + if (next_tick <= now) next_tick = now + period_; // never burst to catch up + } + } +} + +void FollowerExchange::handle_reply_(const bus::AckResponse& ack, bool carried_target) { + const auto& d = ack.data; + protocol::ExchangeResponseHeader h{}; + bool ok = !ack.is_nack && d.size() >= sizeof(h); + if (ok) { + std::memcpy(&h, d.data(), sizeof(h)); + ok = h.version == protocol::EXCHANGE_VERSION; + } + std::size_t pos = sizeof(h); + const std::size_t motor = (ok && (h.flags & protocol::ExchangeRespFlag::Motor)) + ? protocol::MOTOR_STATUS_V2_SIZE : 0; + const std::size_t imu = ok ? imu_size(h.imu_cmd) : 0; + if (ok && h.imu_count && !imu) ok = false; + if (ok && d.size() != pos + motor + std::size_t(h.imu_count) * imu) ok = false; + + std::vector frames; + if (ok) { + auto frame = [&](protocol::Cmd cmd, std::size_t n) { + bus::Frame f{}; + f.addr = protocol::Address::MCU; + f.seq = ack.seq; + f.type = protocol::FrameType::DATA; + f.cmd = cmd; + f.payload.assign(d.begin() + static_cast(pos), + d.begin() + static_cast(pos + n)); + f.receive_time = ack.receive_time; + pos += n; + frames.push_back(std::move(f)); + }; + if (motor) frame(protocol::Cmd::GetMotorStatus, motor); + for (unsigned i = 0; i < h.imu_count; ++i) frame(static_cast(h.imu_cmd), imu); + } + for (auto& f : frames) t_.inject_data(std::move(f)); + + std::lock_guard lk(mu_); + if (ack.is_nack) { ++stats_.nacks; return; } + if (!ok) { ++stats_.malformed; return; } + if (motor) ++stats_.motor_samples; + stats_.imu_samples += h.imu_count; + stats_.imu_dropped += h.imu_dropped; + if (carried_target) { + stats_.last_target_result = h.target_result; + if (h.target_result != 0) ++stats_.targets_rejected; + } +} + +} // namespace xense::taccap::detail diff --git a/cpp/src/follower_exchange.hpp b/cpp/src/follower_exchange.hpp new file mode 100644 index 0000000..3439d06 --- /dev/null +++ b/cpp/src/follower_exchange.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// Host side of the follower half-duplex exchange (Cmd::SlaveExchange). +// +// Why: with the firmware streaming on its own clock, a host write that lands +// while the follower is transmitting loses the overlapping uplink bytes +// (tc-gu-01 docs/TX_COLLISION_RETRY.md). In StreamMode::Polled the follower +// pushes nothing; this loop is the only thing that asks it to talk, and the +// transport's half-duplex turn keeps every other request out of the way while +// a reply may be on the wire. The line therefore carries one direction at a +// time by construction, not by luck. +// +// Cadence: one telemetry exchange per period. A target submitted between +// ticks goes out at once in a target-only exchange (header-only reply), so a +// controller that reacts to a status sample keeps today's ~1 ms latency +// instead of waiting a whole period. A lost reply is retried with the same seq +// (the firmware replays its cached reply, so no IMU sample is lost); a request +// still unanswered after the retries keeps its target for the next exchange +// unless a newer one superseded it; targets are absolute set-points, so +// applying one twice is harmless. +// +// Replies are split back into the DATA frames a stream would have carried +// (GetMotorStatus, GetImu/GetImuV2) and injected into the transport's normal +// dispatch, so subscribers and the controllers' control lane see the same +// samples as in streaming mode. + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace xense::taccap::detail { + +class FollowerExchange { +public: + FollowerExchange(bus::Transport& transport, std::chrono::microseconds period, + std::chrono::milliseconds timeout); + ~FollowerExchange(); + + FollowerExchange(const FollowerExchange&) = delete; + FollowerExchange& operator=(const FollowerExchange&) = delete; + + // Latest target wins. Only the four Motor*Ctrl commands are accepted; + // anything else throws IoError(EINVAL). After stop() it is ignored. + void submit(protocol::Cmd cmd, std::vector payload); + + // Idempotent; joins the worker. + void stop() noexcept; + + ExchangeStats stats() const; + +private: + struct Target { + protocol::ExchangeTarget kind; + std::vector payload; + }; + + void run_(); + void handle_reply_(const bus::AckResponse& ack, bool carried_target); + + bus::Transport& t_; + const std::chrono::microseconds period_; + const std::chrono::milliseconds timeout_; + + mutable std::mutex mu_; + detail::MonotonicCondition cv_; // monotonic even on old-sysroot toolchains + std::optional pending_; + bool stop_ = false; + ExchangeStats stats_{}; + std::thread worker_; +}; + +} // namespace xense::taccap::detail diff --git a/cpp/src/follower_gripper.cpp b/cpp/src/follower_gripper.cpp index e79a313..4c2257e 100644 --- a/cpp/src/follower_gripper.cpp +++ b/cpp/src/follower_gripper.cpp @@ -3,6 +3,7 @@ #include #include "wrist_fisheye.hpp" #include "stream_rate.hpp" +#include "follower_exchange.hpp" #include #include #include @@ -295,6 +296,7 @@ FollowerGripper::~FollowerGripper() { } void FollowerGripper::close() noexcept { + stop_exchange_(); if (!t_.is_open()) return; if (t_.is_running()) { try { stop_streaming(); } catch (...) {} @@ -318,18 +320,29 @@ std::unique_ptr FollowerGripper::open() { } void FollowerGripper::start_streaming(unsigned motor_hz) { - if (streaming_) return; - - // Motor status is the only source the follower firmware emits; everything - // else in its stream task is compiled out on this role. So there is exactly - // one rate to honour, and a rate of 0 has nowhere useful to go. - if (motor_hz == 0) { - throw IoError("FollowerGripper::start_streaming: motor_hz is 0, and " - "motor status is the only source a follower streams — " - "pass a non-zero rate, or just don't start the stream", + start_streaming(motor_hz, 0); +} + +void FollowerGripper::start_streaming(unsigned motor_hz, unsigned imu_hz) { + if (imu_hz > 100) throw IoError("Follower IMU rate must be 0..100 Hz", EINVAL); + if (streaming_) { + const auto requested = detail::stream_source_mask(imu_hz, 0, motor_hz); + if ((requested & streaming_sources_) != requested) + throw IoError("FollowerGripper: stop streaming before adding sources", EBUSY); + return; + } + + // Zero rates clear source bits, rather than selecting firmware defaults. + if (motor_hz == 0 && imu_hz == 0) { + throw IoError("FollowerGripper::start_streaming: both rates are 0", EINVAL); } + // Probe before StopStream: old firmware must fail explicitly without + // disturbing a stream left by another client. Never infer support from a + // shared firmware version number (leader/follower have separate features). + if (imu_hz) (void)imu_.read_once(std::chrono::milliseconds{500}); + // Drain the firmware queue from any previous host process — same // rationale as LeaderGripper::start_streaming. try { @@ -341,16 +354,17 @@ void FollowerGripper::start_streaming(unsigned motor_hz) { detail::kMotorMaxRateHz); protocol::StreamConfig sc{}; - // Only the MotorStatus bit: a rate alone would not turn a source on, and a - // set bit with rate 0 would stream at the firmware's 100Hz default rather - // than staying off. See stream_rate.hpp. - sc.source_mask = protocol::StreamSrc::MotorStatus; + // Legacy calls keep only MotorStatus; IMU is always explicit opt-in. + sc.source_mask = detail::stream_source_mask(imu_hz, 0, motor_hz); + const auto logical_sources = sc.source_mask; + if (imu_hz) sc.source_mask = (sc.source_mask & ~protocol::StreamSrc::Imu) | imu_.negotiate_time(); sc.mode = static_cast(protocol::StreamMode::Separate); - sc.imu_rate = 0; + sc.imu_rate = static_cast(imu_hz); sc.encoder_rate = 0; sc.eskin_rate = 0; sc.motor_rate = static_cast(motor_hz); sc.output_iface = static_cast(protocol::StreamInterface::Uart); + if (imu_hz) detail::warn_if_rate_adjusted("FollowerGripper", "IMU", imu_hz, 100); auto wire = protocol::encode(sc); auto ack = t_.send_cmd(protocol::Cmd::StartStream, wire); @@ -359,10 +373,67 @@ void FollowerGripper::start_streaming(unsigned motor_hz) { protocol::to_string(ack.error_code)); } + streaming_sources_ = logical_sources; + streaming_ = true; +} + +void FollowerGripper::start_exchange(unsigned motor_hz, unsigned imu_hz, + unsigned exchange_hz) { + if (imu_hz > 100) throw IoError("Follower IMU rate must be 0..100 Hz", EINVAL); + if (motor_hz > 100) throw IoError("Follower motor status rate must be 0..100 Hz", EINVAL); + if (motor_hz == 0 && imu_hz == 0) + throw IoError("FollowerGripper::start_exchange: both rates are 0", EINVAL); + if (exchange_hz == 0) exchange_hz = std::max({motor_hz, imu_hz, 1u}); + if (exchange_hz > 500) throw IoError("FollowerGripper::start_exchange: exchange_hz must be <= 500", EINVAL); + if (streaming_) throw IoError("FollowerGripper: stop streaming before start_exchange", EBUSY); + + if (imu_hz) (void)imu_.read_once(std::chrono::milliseconds{500}); + try { + t_.send_cmd(protocol::Cmd::StopStream, {}, std::chrono::milliseconds(500)); + } catch (...) { /* expected when fw is already idle */ } + + protocol::StreamConfig sc{}; + sc.source_mask = detail::stream_source_mask(imu_hz, 0, motor_hz); + const auto logical_sources = sc.source_mask; + if (imu_hz) sc.source_mask = (sc.source_mask & ~protocol::StreamSrc::Imu) | imu_.negotiate_time(); + sc.mode = static_cast(protocol::StreamMode::Polled); + sc.imu_rate = static_cast(imu_hz); + sc.motor_rate = static_cast(motor_hz); + sc.output_iface = static_cast(protocol::StreamInterface::Uart); + auto ack = t_.send_cmd(protocol::Cmd::StartStream, protocol::encode(sc)); + if (ack.is_nack) { + throw ProtocolError(std::string("FollowerGripper::start_exchange NACK (firmware " + "without polled mode / 0x5D?): ") + protocol::to_string(ack.error_code)); + } + + t_.set_half_duplex(true); + // A stuck reply must not hold the link longer than a couple of periods. + const auto period = std::chrono::microseconds(1'000'000 / exchange_hz); + const auto timeout = std::chrono::milliseconds(std::max( + 5, std::chrono::duration_cast(2 * period).count())); + exchange_ = std::make_shared(t_, period, timeout); + std::weak_ptr weak = exchange_; + motor_.set_target_router_([weak](protocol::Cmd cmd, std::vector payload) { + if (auto ex = weak.lock()) ex->submit(cmd, std::move(payload)); + }); + streaming_sources_ = logical_sources; streaming_ = true; } +ExchangeStats FollowerGripper::exchange_stats() const { + return exchange_ ? exchange_->stats() : ExchangeStats{}; +} + +void FollowerGripper::stop_exchange_() noexcept { + if (!exchange_) return; + motor_.set_target_router_({}); + exchange_->stop(); + exchange_.reset(); + t_.set_half_duplex(false); +} + void FollowerGripper::stop_streaming() { + stop_exchange_(); if (!streaming_) return; streaming_ = false; try { diff --git a/cpp/src/force_position_controller.cpp b/cpp/src/force_position_controller.cpp index 028e48f..1238d98 100644 --- a/cpp/src/force_position_controller.cpp +++ b/cpp/src/force_position_controller.cpp @@ -182,6 +182,15 @@ bool has_serious_fault(uint16_t status) noexcept { return (status & mask) != 0; } +const char* status_fault_reason(const MotorStatusSample& sample, float torque_limit) noexcept { + if (!std::isfinite(sample.actual_pos) || !std::isfinite(sample.actual_vel) || + !std::isfinite(sample.actual_torque)) return "non-finite motor status"; + if (has_serious_fault(sample.status)) return "motor status reports a fault"; + if (std::abs(sample.actual_torque) > torque_limit + 1e-4f) + return "measured torque exceeded motion_torque_limit_nm"; + return nullptr; +} + } // namespace const char* to_string(ForcePositionState state) noexcept { @@ -770,15 +779,7 @@ protocol::MotorImpedanceCtrl ForcePositionPolicy::travel_track_( protocol::MotorImpedanceCtrl ForcePositionPolicy::step( const MotorStatusSample& sample, std::chrono::steady_clock::time_point now) { - if (!std::isfinite(sample.actual_pos) || !std::isfinite(sample.actual_vel) || - !std::isfinite(sample.actual_torque)) { - fail("non-finite motor status"); - } else if (has_serious_fault(sample.status)) { - fail("motor status reports a fault"); - } else if (std::abs(sample.actual_torque) > - cfg_.motion_torque_limit_nm + 1e-4f) { - fail("measured torque exceeded motion_torque_limit_nm"); - } + if (const char* reason = status_fault_reason(sample, cfg_.motion_torque_limit_nm)) fail(reason); if (state_ == ForcePositionState::Fault || state_ == ForcePositionState::Idle) { arrived_ = false; @@ -1161,7 +1162,9 @@ void ForcePositionController::start() { policy_->reset(initial, now); queue_.clear(); latest_ = initial; - latest_time_ = now; + latest_time_ = initial.host_time; + pending_status_fault_ = status_fault_reason(initial, cfg_.motion_torque_limit_nm); + feedback_epoch_ = now; have_sample_ = true; observation_ = GripperObservation{}; observation_.valid = true; @@ -1178,8 +1181,16 @@ void ForcePositionController::start() { stall_limit_nm_ = stall_limit_nm; } - sub_ = g_.motor().on_status( - [this](const MotorStatusSample& sample) { on_status_(sample); }); + try { + sub_ = g_.motor().on_control_status_( + [this](const MotorStatusSample& sample) { on_status_(sample); }); + } catch (...) { + std::lock_guard lock(mu_); + stop_requested_ = true; + have_sample_ = false; + observation_.valid = false; + throw; + } sub_active_ = true; try { start_motor_stream_(); @@ -1259,7 +1270,6 @@ void ForcePositionController::stop() { } void ForcePositionController::on_status_(const MotorStatusSample& sample) { - const auto now = std::chrono::steady_clock::now(); { std::lock_guard lk(mu_); // stop() unsubscribes only AFTER the zero frame and the disable ACK, so @@ -1268,8 +1278,21 @@ void ForcePositionController::on_status_(const MotorStatusSample& sample) { // observation valid again -- ImpedanceControllerPty.StopCommandsZeroTorque // caught it roughly one run in five under CPU load. if (stop_requested_) return; + const auto now = std::chrono::steady_clock::now(); + if (sample.host_time.time_since_epoch().count() <= 0 || + sample.host_time > now || sample.host_time < feedback_epoch_) return; + // Preserve safety evidence before latest-value coalescing/age filtering. + // No logging, allocation or policy execution on the control worker. + if (!pending_status_fault_) + pending_status_fault_ = status_fault_reason(sample, cfg_.motion_torque_limit_nm); + if (pending_status_fault_) { + step_requested_ = true; + cv_.notify_one(); + } + if ((have_sample_ && sample.host_time < latest_time_) || + now - sample.host_time >= std::chrono::milliseconds(cfg_.status_timeout_ms)) return; latest_ = sample; - latest_time_ = now; + latest_time_ = sample.host_time; have_sample_ = true; observation_.valid = true; observation_.position = map_.to_position(sample.actual_pos); @@ -1303,13 +1326,11 @@ void ForcePositionController::enqueue_(const QueuedCommand& c) { // Caller commands are QUEUED, not applied here. // -// Applying them on the caller's thread also submitted on the caller's thread, -// at whatever moment that thread happened to call in. The MCU drops bytes out -// of the middle of a status frame it is transmitting if host->MCU traffic -// overlaps it, so an off-phase write costs a telemetry frame. Every submit now -// happens in run_() on the status-frame doorbell, inside the ~9.8 ms the MCU -// is known to be idle. The cost is that a command takes effect on the next status frame -// instead of instantly: at motor_stream_hz = 100 that is under 10 ms. +// Applying targets on the controller worker keeps control state ordered with +// the latest feedback. A received status frame does NOT establish a UART idle +// window: IMU/ACK traffic can follow it immediately. This is feedback-paced, +// not a hard 100 Hz timer or a guarantee of loss-free full-duplex transport. +// Use motor.target_writes()/delivery_snapshot() to observe gaps and acceptance. // // QUEUED IN ORDER AND STAMPED HERE (0.4.2). Each target carries the time this // call was made, on the caller's thread, because the policy builds its @@ -1372,6 +1393,8 @@ void ForcePositionController::reset() { throw std::logic_error("ForcePositionController::reset called before start"); } policy_->reset(latest_, std::chrono::steady_clock::now()); + pending_status_fault_ = nullptr; + feedback_epoch_ = std::chrono::steady_clock::now(); // The cached sample may still carry the fault bit that was true immediately // before Motor::clear_fault() ACKed. Wait for the next streamed status rather // than re-faulting the freshly reset policy on stale evidence. @@ -1424,6 +1447,11 @@ void ForcePositionController::run_() { command_woke_ = false; const auto now = std::chrono::steady_clock::now(); if (!policy_ || !have_sample_) continue; + if (pending_status_fault_) { + if (policy_->state() != ForcePositionState::Fault) policy_->fail(pending_status_fault_); + pending_status_fault_ = nullptr; + step_requested_ = true; + } // Staleness is checked unconditionally. Doing it only in the // !step_requested_ branch let set_target()/release()/hold_position() diff --git a/cpp/src/impedance_controller.cpp b/cpp/src/impedance_controller.cpp index d85052f..2f6b008 100644 --- a/cpp/src/impedance_controller.cpp +++ b/cpp/src/impedance_controller.cpp @@ -115,6 +115,15 @@ bool has_serious_fault(uint16_t status) noexcept { return (status & mask) != 0; } +const char* status_fault_reason(const MotorStatusSample& sample, float torque_limit) noexcept { + if (!std::isfinite(sample.actual_pos) || !std::isfinite(sample.actual_vel) || + !std::isfinite(sample.actual_torque)) return "non-finite motor status"; + if (has_serious_fault(sample.status)) return "motor status reports a fault"; + if (std::abs(sample.actual_torque) > torque_limit + 1e-4f) + return "measured torque exceeded the motor's torque range"; + return nullptr; +} + } // namespace const char* to_string(ImpedanceState state) noexcept { @@ -251,14 +260,7 @@ protocol::MotorImpedanceCtrl ImpedancePolicy::zero_(const MotorStatusSample& s) protocol::MotorImpedanceCtrl ImpedancePolicy::step( const MotorStatusSample& sample, std::chrono::steady_clock::time_point now) { - if (!std::isfinite(sample.actual_pos) || !std::isfinite(sample.actual_vel) || - !std::isfinite(sample.actual_torque)) { - fail("non-finite motor status"); - } else if (has_serious_fault(sample.status)) { - fail("motor status reports a fault"); - } else if (std::abs(sample.actual_torque) > cfg_.peak_torque_nm + 1e-4f) { - fail("measured torque exceeded the motor's torque range"); - } + if (const char* reason = status_fault_reason(sample, cfg_.peak_torque_nm)) fail(reason); if (state_ == ImpedanceState::Fault || state_ == ImpedanceState::Idle) { return zero_(sample); @@ -490,7 +492,9 @@ void ImpedanceController::start() { policy_ = std::make_unique(map_, cfg_); policy_->reset(initial); // seeds the target with where we are latest_ = initial; - latest_time_ = now; + latest_time_ = initial.host_time; + pending_status_fault_ = status_fault_reason(initial, cfg_.peak_torque_nm); + feedback_epoch_ = now; have_sample_ = true; observation_ = GripperObservation{}; observation_.valid = true; @@ -506,8 +510,16 @@ void ImpedanceController::start() { device_limit_nm_ = device_limit; } - sub_ = g_.motor().on_status( - [this](const MotorStatusSample& sample) { on_status_(sample); }); + try { + sub_ = g_.motor().on_control_status_( + [this](const MotorStatusSample& sample) { on_status_(sample); }); + } catch (...) { + std::lock_guard lock(mu_); + stop_requested_ = true; + have_sample_ = false; + observation_.valid = false; + throw; + } sub_active_ = true; try { start_motor_stream_(); @@ -575,7 +587,6 @@ void ImpedanceController::stop() { } void ImpedanceController::on_status_(const MotorStatusSample& sample) { - const auto now = std::chrono::steady_clock::now(); { std::lock_guard lk(mu_); // stop() unsubscribes only AFTER the zero frame and the disable ACK, so @@ -584,8 +595,19 @@ void ImpedanceController::on_status_(const MotorStatusSample& sample) { // observation valid again -- ImpedanceControllerPty.StopCommandsZeroTorque // caught it roughly one run in five under CPU load. if (stop_requested_) return; + const auto now = std::chrono::steady_clock::now(); + if (sample.host_time.time_since_epoch().count() <= 0 || + sample.host_time > now || sample.host_time < feedback_epoch_) return; + if (!pending_status_fault_) + pending_status_fault_ = status_fault_reason(sample, cfg_.peak_torque_nm); + if (pending_status_fault_) { + step_requested_ = true; + cv_.notify_one(); + } + if ((have_sample_ && sample.host_time < latest_time_) || + now - sample.host_time >= std::chrono::milliseconds(cfg_.status_timeout_ms)) return; latest_ = sample; - latest_time_ = now; + latest_time_ = sample.host_time; have_sample_ = true; observation_.valid = true; observation_.position = map_.to_position(sample.actual_pos); @@ -640,6 +662,8 @@ void ImpedanceController::reset() { throw std::logic_error("ImpedanceController::reset called before start"); } policy_->reset(latest_); + pending_status_fault_ = nullptr; + feedback_epoch_ = std::chrono::steady_clock::now(); // The cached sample may still carry the fault bit that was true immediately // before Motor::clear_fault() ACKed. Wait for the next streamed status // rather than re-faulting the freshly reset policy on stale evidence. @@ -689,6 +713,11 @@ void ImpedanceController::run_() { command_woke_ = false; const auto now = std::chrono::steady_clock::now(); if (!policy_ || !have_sample_) continue; + if (pending_status_fault_) { + if (policy_->state() != ImpedanceState::Fault) policy_->fail(pending_status_fault_); + pending_status_fault_ = nullptr; + step_requested_ = true; + } const bool stale = now - latest_time_ >= timeout; if (stale) { diff --git a/cpp/src/leader_exchange.cpp b/cpp/src/leader_exchange.cpp new file mode 100644 index 0000000..e9f0373 --- /dev/null +++ b/cpp/src/leader_exchange.cpp @@ -0,0 +1,168 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 + +#include "leader_exchange.hpp" + +#include + +#include +#include + +namespace xense::taccap::detail { + +namespace { + +using clock = std::chrono::steady_clock; + +// Back-to-back exchanges allowed per period while the firmware reports a +// backlog; a backlog larger than this is left for the next period. +constexpr unsigned kMaxContinuations = 8; +// At most one re-arm attempt in this interval, so a firmware that keeps +// refusing the polled mode is not hammered with StartStream. +constexpr auto kRearmInterval = std::chrono::milliseconds(200); + +} // namespace + +LeaderExchange::LeaderExchange(bus::Transport& transport, + std::chrono::microseconds period, + std::chrono::milliseconds timeout, + std::function rearm) + : t_(transport), period_(period), timeout_(timeout), rearm_(std::move(rearm)) { + worker_ = std::thread([this] { run_(); }); +} + +LeaderExchange::~LeaderExchange() { stop(); } + +void LeaderExchange::stop() noexcept { + { + std::lock_guard lk(mu_); + stop_ = true; + } + cv_.notify_all(); + if (worker_.joinable() && worker_.get_id() != std::this_thread::get_id()) worker_.join(); +} + +LeaderExchangeStats LeaderExchange::stats() const { + std::lock_guard lk(mu_); + return stats_; +} + +bool LeaderExchange::decode(const std::vector& d, + protocol::LeaderExchangeResponseHeader& h, + std::vector& frames) { + frames.clear(); + if (d.size() < sizeof(h)) return false; + std::memcpy(&h, d.data(), sizeof(h)); + if (h.version != protocol::LEADER_EXCHANGE_VERSION) return false; + std::size_t pos = sizeof(h); + for (unsigned i = 0; i < h.record_count; ++i) { + if (d.size() - pos < 2) { frames.clear(); return false; } + const std::size_t n = d[pos + 1]; + if (d.size() - pos - 2 < n) { frames.clear(); return false; } + bus::Frame f{}; + f.addr = protocol::Address::MCU; + f.type = protocol::FrameType::DATA; + f.cmd = static_cast(d[pos]); + f.payload.assign(d.begin() + static_cast(pos + 2), + d.begin() + static_cast(pos + 2 + n)); + pos += 2 + n; + frames.push_back(std::move(f)); + } + if (pos != d.size()) { frames.clear(); return false; } + return true; +} + +void LeaderExchange::run_() { + protocol::LeaderExchangeRequest req{}; + req.version = protocol::LEADER_EXCHANGE_VERSION; + std::vector wire(sizeof(req)); + std::memcpy(wire.data(), &req, sizeof(req)); + + auto next_tick = clock::now(); + auto last_rearm = clock::now() - kRearmInterval; + unsigned continuations = 0; + std::unique_lock lk(mu_); + while (!stop_) { + cv_.wait_until(lk, next_tick, [&] { return stop_; }); + if (stop_) break; + lk.unlock(); + + const auto started = clock::now(); + std::optional ack; + bool nack = false, link_down = false; + try { + // SameSeq: a retry after a lost reply makes the firmware replay its + // cached reply instead of handing out (and losing) new records. + ack = t_.send_cmd(protocol::Cmd::LeaderExchange, wire, timeout_, bus::RetryMode::SameSeq); + } catch (const NackError&) { + nack = true; + } catch (const TimeoutError&) { + } catch (const std::exception&) { + link_down = !t_.is_running(); + } + const auto rtt_us = static_cast( + std::chrono::duration_cast(clock::now() - started).count()); + + protocol::LeaderExchangeResponseHeader h{}; + std::vector frames; + const bool ok = ack && decode(ack->data, h, frames); + LeaderExchangeStats add{}; + for (auto& f : frames) { + f.seq = ack->seq; + f.receive_time = ack->receive_time; + switch (f.cmd) { + case protocol::Cmd::GetImu: + case protocol::Cmd::GetImuV2: ++add.imu_samples; break; + case protocol::Cmd::GetEncoder: ++add.encoder_samples; break; + case protocol::Cmd::KeyStatus: ++add.key_events; break; + case protocol::Cmd::SensorErrorReport: ++add.sensor_errors; break; + default: ++add.other_records; break; + } + t_.inject_data(std::move(f)); + } + + lk.lock(); + ++stats_.exchanges; + if (continuations) ++stats_.continuation_exchanges; + if (ok) { + stats_.records += h.record_count; + stats_.records_dropped += h.dropped; + stats_.imu_samples += add.imu_samples; + stats_.encoder_samples += add.encoder_samples; + stats_.key_events += add.key_events; + stats_.sensor_errors += add.sensor_errors; + stats_.other_records += add.other_records; + if (!(h.flags & protocol::LeaderExchangeRespFlag::Polled)) ++stats_.not_polled; + stats_.last_rtt_us = rtt_us; + if (rtt_us > stats_.max_rtt_us) stats_.max_rtt_us = rtt_us; + } else if (nack) { + ++stats_.nacks; + } else if (ack) { + ++stats_.malformed; + } else { + ++stats_.timeouts; + } + if (link_down) break; + // Firmware fell back to pushing (lease expired while this host stalled, + // or another process restarted the stream): its pushed frames are being + // dispatched normally meanwhile; ask for the exchange again. + if (ok && !(h.flags & protocol::LeaderExchangeRespFlag::Polled) && rearm_ && + clock::now() - last_rearm >= kRearmInterval) { + last_rearm = clock::now(); + lk.unlock(); + bool rearmed = false; + try { rearm_(); rearmed = true; } catch (const std::exception&) {} + lk.lock(); + if (rearmed) ++stats_.rearms; else ++stats_.rearm_failures; + } + if (ok && h.pending && continuations < kMaxContinuations) { + ++continuations; // backlog: fetch again now, keep the tick + continue; + } + continuations = 0; + next_tick += period_; + const auto now = clock::now(); + if (next_tick <= now) next_tick = now + period_; // never burst to catch up + } +} + +} // namespace xense::taccap::detail diff --git a/cpp/src/leader_exchange.hpp b/cpp/src/leader_exchange.hpp new file mode 100644 index 0000000..eaf1068 --- /dev/null +++ b/cpp/src/leader_exchange.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// Host side of the leader half-duplex exchange (Cmd::LeaderExchange). +// +// Why: same as FollowerExchange. With the leader streaming on its own clock, a +// host write that lands while the leader is transmitting loses the overlapping +// uplink bytes (tc-gu-01 docs/LEADER_EXCHANGE.md). In StreamMode::Polled the +// leader holds every DATA frame it would have pushed (IMU, encoder, key, +// sensor error); this loop is the only thing that asks for them, and the +// transport's half-duplex turn keeps user queries from overlapping a reply. +// +// Cadence: one exchange per period; when the reply says records are still +// pending, the next exchange goes out at once (bounded) instead of waiting. +// A timed-out exchange is retried with the same seq, so the firmware replays +// its cached reply and the records it already handed out are not lost. +// +// Records are re-dispatched as the DATA frames they are, so imu().on_data, +// encoder().on_data, key().on_event and sensor-error subscribers see exactly +// what streaming would have delivered. + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace xense::taccap::detail { + +class LeaderExchange { +public: + // rearm re-sends the polled StartStream; it runs on the worker when a reply + // says the firmware left the exchange (its 1 s lease expired, or another + // process restarted the stream). + LeaderExchange(bus::Transport& transport, std::chrono::microseconds period, + std::chrono::milliseconds timeout, std::function rearm = {}); + ~LeaderExchange(); + + LeaderExchange(const LeaderExchange&) = delete; + LeaderExchange& operator=(const LeaderExchange&) = delete; + + // Idempotent; joins the worker. + void stop() noexcept; + + LeaderExchangeStats stats() const; + + // Split one reply into its header and the DATA frames its records carry. + // False (and no frames) when the reply is malformed. Public for tests. + static bool decode(const std::vector& reply, + protocol::LeaderExchangeResponseHeader& head, + std::vector& frames); + +private: + void run_(); + + bus::Transport& t_; + const std::chrono::microseconds period_; + const std::chrono::milliseconds timeout_; + const std::function rearm_; + + mutable std::mutex mu_; + detail::MonotonicCondition cv_; // monotonic even on old-sysroot toolchains + bool stop_ = false; + LeaderExchangeStats stats_{}; + std::thread worker_; +}; + +} // namespace xense::taccap::detail diff --git a/cpp/src/leader_gripper.cpp b/cpp/src/leader_gripper.cpp index 528401d..4ececcb 100644 --- a/cpp/src/leader_gripper.cpp +++ b/cpp/src/leader_gripper.cpp @@ -1,6 +1,7 @@ // Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 #include +#include "leader_exchange.hpp" #include "wrist_fisheye.hpp" #include "stream_rate.hpp" #include @@ -10,6 +11,7 @@ #include #include +#include #include #include @@ -42,6 +44,39 @@ Camera::Config make_wrist_config(const LeaderGripper::Config& cfg) { return c; } +// The StreamConfig both start_streaming() and start_exchange() send. A rate of +// 0 means "off", and the only way to say that to the firmware is to leave the +// source's mask bit clear -- the rate field alone would fall through to the +// firmware's 100 Hz default. See stream_rate.hpp. +protocol::StreamConfig leader_stream_config(const char* caller, IMU& imu, unsigned imu_hz, + unsigned encoder_hz, protocol::StreamMode mode, + bool imu_v2_time = true) { + const uint16_t mask = detail::stream_source_mask(imu_hz, encoder_hz, 0); + if (mask == 0) { + throw IoError(std::string("LeaderGripper::") + caller + ": both rates are 0, so " + "nothing would be streamed — pass a non-zero rate for at " + "least one source, or just don't start the stream", + EINVAL); + } + detail::warn_if_rate_adjusted("LeaderGripper", "IMU", imu_hz, 0); + detail::warn_if_rate_adjusted("LeaderGripper", "encoder", encoder_hz, 0); + + // Build StreamConfig (12 bytes — see protocol::StreamConfig). + protocol::StreamConfig sc{}; + sc.source_mask = mask; + // imu_v2_time=false keeps the legacy 28 B GetImu frame and skips the time + // session entirely (what SDK <= 0.4.4 always did). + if (imu_hz && imu_v2_time) + sc.source_mask = (mask & ~protocol::StreamSrc::Imu) | imu.negotiate_time(); + sc.mode = static_cast(mode); + sc.imu_rate = static_cast(imu_hz); + sc.encoder_rate = static_cast(encoder_hz); + sc.eskin_rate = 0; + sc.motor_rate = 0; + sc.output_iface = static_cast(protocol::StreamInterface::Uart); + return sc; +} + } // namespace LeaderGripper::LeaderGripper(const Config& cfg) @@ -129,6 +164,7 @@ LeaderGripper::LeaderGripper(const Config& cfg) LeaderGripper::~LeaderGripper() { try { stop_streaming(); } catch (...) {} + stop_exchange_(); } void LeaderGripper::close() noexcept { @@ -136,6 +172,7 @@ void LeaderGripper::close() noexcept { if (t_.is_running()) { try { stop_streaming(); } catch (...) {} } + stop_exchange_(); if (wrist_) { try { wrist_->stop(); } catch (...) {} } @@ -154,7 +191,7 @@ std::unique_ptr LeaderGripper::open() { return std::make_unique(cfg); } -void LeaderGripper::start_streaming(unsigned imu_hz, unsigned encoder_hz) { +void LeaderGripper::start_streaming(unsigned imu_hz, unsigned encoder_hz, bool imu_v2_time) { if (streaming_) return; // Best-effort reset of the firmware streaming state. If a previous host @@ -168,29 +205,8 @@ void LeaderGripper::start_streaming(unsigned imu_hz, unsigned encoder_hz) { std::chrono::milliseconds(500)); } catch (...) { /* expected when fw is already idle */ } - // A rate of 0 means "off", and the only way to say that to the firmware - // is to leave the source's mask bit clear — the rate field alone would - // fall through to the firmware's 100 Hz default. See stream_rate.hpp. - const uint16_t mask = detail::stream_source_mask(imu_hz, encoder_hz, 0); - if (mask == 0) { - throw IoError("LeaderGripper::start_streaming: both rates are 0, so " - "nothing would be streamed — pass a non-zero rate for at " - "least one source, or just don't start the stream", - EINVAL); - } - detail::warn_if_rate_adjusted("LeaderGripper", "IMU", imu_hz, 0); - detail::warn_if_rate_adjusted("LeaderGripper", "encoder", encoder_hz, 0); - - // Build StreamConfig (12 bytes — see protocol::StreamConfig). - protocol::StreamConfig sc{}; - sc.source_mask = mask; - sc.mode = static_cast(protocol::StreamMode::Separate); - sc.imu_rate = static_cast(imu_hz); - sc.encoder_rate = static_cast(encoder_hz); - sc.eskin_rate = 0; - sc.motor_rate = 0; - sc.output_iface = static_cast(protocol::StreamInterface::Uart); - + const auto sc = leader_stream_config("start_streaming", imu_, imu_hz, encoder_hz, + protocol::StreamMode::Separate, imu_v2_time); auto wire = protocol::encode(sc); auto ack = t_.send_cmd(protocol::Cmd::StartStream, wire); if (ack.is_nack) { @@ -204,9 +220,79 @@ void LeaderGripper::start_streaming(unsigned imu_hz, unsigned encoder_hz) { streaming_ = true; } +bool LeaderGripper::start_exchange(unsigned imu_hz, unsigned encoder_hz, + unsigned exchange_hz, bool fallback_to_streaming) { + if (exchange_) return true; + if (streaming_) throw IoError("LeaderGripper: stop streaming before start_exchange", EBUSY); + if (exchange_hz == 0) exchange_hz = detail::leader_default_exchange_hz(imu_hz, encoder_hz); + if (exchange_hz > 500) + throw IoError("LeaderGripper::start_exchange: exchange_hz must be <= 500", EINVAL); + if (detail::stream_source_mask(imu_hz, encoder_hz, 0) == 0) + (void)leader_stream_config("start_exchange", imu_, imu_hz, encoder_hz, + protocol::StreamMode::Polled); // throws EINVAL + + try { + t_.send_cmd(protocol::Cmd::StopStream, {}, std::chrono::milliseconds(500)); + } catch (...) { /* expected when fw is already idle */ } + + // Probe with the exchange itself: firmware without it answers InvalidCmd + // (the device version is not a capability signal -- it stays 1.2.6). + auto unsupported = [&](const std::string& why) { + if (!fallback_to_streaming) + throw ProtocolError("LeaderGripper::start_exchange: " + why); + logger()->info("LeaderGripper: {}; falling back to start_streaming", why); + start_streaming(imu_hz, encoder_hz); + return false; + }; + protocol::LeaderExchangeRequest req{}; + req.version = protocol::LEADER_EXCHANGE_VERSION; + std::vector probe(sizeof(req)); + std::memcpy(probe.data(), &req, sizeof(req)); + try { + t_.send_cmd(protocol::Cmd::LeaderExchange, probe, std::chrono::milliseconds(500)); + } catch (const NackError& e) { + if (e.code() != static_cast(protocol::ErrorCode::InvalidCmd)) throw; + return unsupported("firmware without the leader exchange (0x5E)"); + } + + const auto sc = leader_stream_config("start_exchange", imu_, imu_hz, encoder_hz, + protocol::StreamMode::Polled); + try { + t_.send_cmd(protocol::Cmd::StartStream, protocol::encode(sc)); + } catch (const NackError& e) { + return unsupported(std::string("firmware rejected the polled stream mode: ") + e.what()); + } + + t_.set_half_duplex(true); + // A stuck reply must not hold the link longer than a couple of periods. + const auto period = std::chrono::microseconds(1'000'000 / exchange_hz); + const auto timeout = std::chrono::milliseconds(std::max( + 5, std::chrono::duration_cast(2 * period).count())); + // Re-enter the exchange after the firmware's lease fallback with the same + // configuration (sources, rates and the negotiated IMU format). + auto rearm = [this, wire = protocol::encode(sc)] { + t_.send_cmd(protocol::Cmd::StartStream, wire, std::chrono::milliseconds(200)); + }; + exchange_ = std::make_shared(t_, period, timeout, std::move(rearm)); + streaming_ = true; + return true; +} + +LeaderExchangeStats LeaderGripper::exchange_stats() const { + return exchange_ ? exchange_->stats() : LeaderExchangeStats{}; +} + +void LeaderGripper::stop_exchange_() noexcept { + if (!exchange_) return; + exchange_->stop(); + exchange_.reset(); + t_.set_half_duplex(false); +} + void LeaderGripper::stop_streaming() { if (!streaming_) return; streaming_ = false; + stop_exchange_(); try { t_.send_cmd(protocol::Cmd::StopStream, {}, std::chrono::milliseconds{500}); } catch (...) { diff --git a/cpp/src/protocol/codec.cpp b/cpp/src/protocol/codec.cpp index 2f2858d..f5371cf 100644 --- a/cpp/src/protocol/codec.cpp +++ b/cpp/src/protocol/codec.cpp @@ -238,6 +238,14 @@ UartStats decode_uart_stats(const uint8_t* data, std::size_t len) { return out; } +UartRxErrors decode_uart_rx_errors(const uint8_t* data, std::size_t len) { + if (!data || len != 72) + throw ProtocolError("decode UartRxErrors: expected 72-byte extended reply"); + UartRxErrors out{}; + std::memcpy(&out, data + 44, sizeof(out)); + return out; +} + LogConfig decode_log_config(const uint8_t* data, std::size_t len) { return pod_from_bytes(data, len, "LogConfig"); } diff --git a/cpp/src/protocol/commands.cpp b/cpp/src/protocol/commands.cpp index e306dea..6cae760 100644 --- a/cpp/src/protocol/commands.cpp +++ b/cpp/src/protocol/commands.cpp @@ -36,6 +36,9 @@ const char* to_string(Cmd c) noexcept { case Cmd::Ws2812Effect: return "Ws2812Effect"; case Cmd::GetImu: return "GetImu"; + case Cmd::TimeSession: return "TimeSession"; + case Cmd::GetImuV2: return "GetImuV2"; + case Cmd::TxTrace: return "TxTrace"; case Cmd::GetEncoder: return "GetEncoder"; case Cmd::GetEskin1: return "GetEskin1"; case Cmd::GetEskin2: return "GetEskin2"; @@ -83,6 +86,8 @@ const char* to_string(Cmd c) noexcept { case Cmd::SetMotorModel: return "SetMotorModel"; case Cmd::MotorCanExtXfer: return "MotorCanExtXfer"; case Cmd::SetMotorFwVersion: return "SetMotorFwVersion"; + case Cmd::SlaveExchange: return "SlaveExchange"; + case Cmd::LeaderExchange: return "LeaderExchange"; case Cmd::GetUartStats: return "GetUartStats"; case Cmd::SetLogConfig: return "SetLogConfig"; diff --git a/cpp/src/stream_rate.hpp b/cpp/src/stream_rate.hpp index 552dc11..cc35e1b 100644 --- a/cpp/src/stream_rate.hpp +++ b/cpp/src/stream_rate.hpp @@ -86,4 +86,12 @@ inline void warn_if_rate_adjusted(const char* who, const char* source, } } +// LeaderGripper::start_exchange() default exchange rate: the data rate, capped at +// 200 Hz. The leader samples IMU and encoder at 100 Hz whatever is requested, so +// a 1000 Hz request must not turn into 1000 exchanges/s (it used to throw). +inline unsigned leader_default_exchange_hz(unsigned imu_hz, unsigned encoder_hz) { + const unsigned hz = imu_hz > encoder_hz ? imu_hz : encoder_hz; + return hz < 1 ? 1 : (hz > 200 ? 200 : hz); +} + } // namespace xense::taccap::detail diff --git a/cpp/tests/CMakeLists.txt b/cpp/tests/CMakeLists.txt index c0fe7fb..6fb7851 100644 --- a/cpp/tests/CMakeLists.txt +++ b/cpp/tests/CMakeLists.txt @@ -19,6 +19,7 @@ add_executable(taccap_unit_tests test_ota_retry_pty.cpp test_python_compat.cpp test_stuff.cpp + test_transport_reliability.cpp test_transport.cpp test_transport_concurrent_write.cpp test_diagnostics_codec.cpp @@ -36,6 +37,12 @@ add_executable(taccap_unit_tests test_crc32_boundaries.cpp test_log.cpp test_stream_rate.cpp + test_follower_imu_pty.cpp + test_imu_motor_concurrency.cpp + test_imu_time.cpp + test_follower_exchange_pty.cpp + test_leader_exchange_pty.cpp + test_monotonic_cv.cpp ) # stream_rate.hpp is a private helper under src/ (not an installed header), diff --git a/cpp/tests/fake_follower.hpp b/cpp/tests/fake_follower.hpp index 1ea620d..ad4f386 100644 --- a/cpp/tests/fake_follower.hpp +++ b/cpp/tests/fake_follower.hpp @@ -82,12 +82,49 @@ class FakeFollower { status_.status = bits; } + // Next polling ACK can carry a fault while subsequent DATA is healthy. + void set_next_polled_status_word(uint16_t bits) { + std::lock_guard lk(mu_); + next_polled_status_word_ = bits; + } + std::optional last_submit() const { std::lock_guard lk(mu_); return last_submit_; } unsigned submit_count() const { return submits_.load(); } unsigned disable_count() const { return disables_.load(); } + void set_imu_supported(bool on) { imu_supported_.store(on); } + void set_imu_v2_supported(bool on) { imu_v2_supported_.store(on); imu_supported_.store(on); } + unsigned imu_queries() const { return imu_queries_.load(); } + unsigned stream_stops() const { return stream_stops_.load(); } + tp::StreamConfig stream_config() const { + std::lock_guard lk(mu_); + return stream_config_; + } + // ---- Half-duplex exchange (StreamMode::Polled + Cmd::SlaveExchange) ---- + // Firmware older than the polled mode NACKs StartStream mode 2. + void set_exchange_supported(bool on) { exchange_supported_.store(on); } + bool polled() const { return polled_.load(); } + unsigned exchanges() const { return exchanges_.load(); } + unsigned telemetry_exchanges() const { return telemetry_exchanges_.load(); } + unsigned exchange_targets() const { return exchange_targets_.load(); } + // Hold each exchange reply this long, and count any host frame that + // arrives meanwhile: a half-duplex host never writes while waiting. + void set_exchange_reply_delay_ms(int ms) { exchange_delay_ms_.store(ms); } + unsigned overlaps() const { return overlaps_.load(); } + // Swallow the next n exchange requests (the target is still applied), + // so the host sees timeouts. + void drop_exchange_replies(unsigned n) { drop_replies_.store(n); } + // Build (and cache) the next n exchange replies -- consuming their IMU + // samples -- but lose them on the wire: what a host stall looks like. + void lose_exchange_replies(unsigned n) { drop_after_build_.store(n); } + unsigned exchange_replays() const { return exchange_replays_.load(); } + // Make the next exchange reply's target_result this ErrorCode byte. + void reject_next_exchange_target(uint8_t err) { reject_next_.store(err); } + // Non-exchange frames seen while polled (heartbeats, queries, etc.). + unsigned other_frames_while_polled() const { return other_while_polled_.load(); } + // Make 0x5C store something other than what was sent. void set_corrupt_fw_ver_writes(bool on) { corrupt_fw_ver_writes_.store(on); } @@ -158,14 +195,39 @@ class FakeFollower { if (streaming_.load() && !frozen_.load() && now >= next_status) { next_status = now + std::chrono::milliseconds(10); std::lock_guard lk(mu_); - pty_.send_data(0, tp::Cmd::GetMotorStatus, + if (stream_config_.source_mask & tp::StreamSrc::MotorStatus) + pty_.send_data(0, tp::Cmd::GetMotorStatus, pod_bytes(&status_, sizeof(status_))); + if (stream_config_.source_mask & tp::StreamSrc::Imu) { + tp::ImuData imu{}; + imu.accel_z = 1000; + pty_.send_data(0, tp::Cmd::GetImu, pod_bytes(&imu, sizeof(imu))); + } + if (stream_config_.source_mask & tp::StreamSrc::ImuV2) + pty_.send_data(0,tp::Cmd::GetImuV2,imu_v2_packet_()); } } } void handle_(const xense::taccap::bus::Frame& f) { + if (polled_.load() && f.cmd != tp::Cmd::SlaveExchange) ++other_while_polled_; switch (f.cmd) { + case tp::Cmd::SlaveExchange: { + // Firmware command layer: a byte-identical repeat (same seq, cmd, + // payload) replays the cached reply instead of re-running it. + if (last_exchange_ && last_exchange_->seq == f.seq && last_exchange_->payload == f.payload) { + ++exchange_replays_; + if (last_exchange_reply_) pty_.send_response(f.seq, f.cmd, *last_exchange_reply_); + return; + } + // drop_exchange_replies() models a request that never reached the + // firmware (nothing cached, a retry runs fresh); lose_exchange_replies() + // a reply lost on the wire (cached, a retry replays it). + last_exchange_reply_.reset(); + handle_exchange_(f); + if (last_exchange_reply_) last_exchange_ = f; else last_exchange_.reset(); + return; + } case tp::Cmd::GetVersion: { const tp::FirmwareVersion v{fw_major_, fw_minor_, fw_patch_, 0}; pty_.send_response(f.seq, f.cmd, pod_bytes(&v, sizeof(v))); @@ -317,15 +379,60 @@ class FakeFollower { } case tp::Cmd::GetMotorStatus: { std::lock_guard lk(mu_); + auto reply = status_; + if (next_polled_status_word_) { + reply.status = *next_polled_status_word_; + next_polled_status_word_.reset(); + } pty_.send_response(f.seq, f.cmd, - pod_bytes(&status_, sizeof(status_))); + pod_bytes(&reply, sizeof(reply))); return; } - case tp::Cmd::StartStream: - streaming_.store(true); + case tp::Cmd::TimeSession: { + if(!imu_v2_supported_.load()) {pty_.send_nack(f.seq,tp::ErrorCode::InvalidCmd);return;} + if(f.payload.size()!=8) {pty_.send_nack(f.seq,tp::ErrorCode::LengthMismatch);return;} + uint64_t proposed=0; std::memcpy(&proposed,f.payload.data(),8); + if(!imu_session_) imu_session_=proposed; + uint64_t reply[2]={imu_time_,imu_session_}; + pty_.send_response(f.seq,f.cmd,pod_bytes(reply,sizeof(reply)));return; + } + case tp::Cmd::GetImuV2: { + if(!imu_v2_supported_.load()) {pty_.send_nack(f.seq,tp::ErrorCode::InvalidCmd);return;} + pty_.send_response(f.seq,f.cmd,imu_v2_packet_());return; + } + case tp::Cmd::GetImu: { + ++imu_queries_; + if (!imu_supported_.load()) { + pty_.send_nack(f.seq, tp::ErrorCode::InvalidCmd); + } else { + tp::ImuData imu{}; + imu.accel_z = 1000; + pty_.send_response(f.seq, f.cmd, pod_bytes(&imu, sizeof(imu))); + } + return; + } + case tp::Cmd::StartStream: { + std::lock_guard lk(mu_); + if (f.payload.size() != sizeof(stream_config_)) { + pty_.send_nack(f.seq, tp::ErrorCode::LengthMismatch); + return; + } + tp::StreamConfig requested{}; + std::memcpy(&requested, f.payload.data(), sizeof(requested)); + const bool want_polled = requested.mode == static_cast(tp::StreamMode::Polled); + if (want_polled && !exchange_supported_.load()) { + pty_.send_nack(f.seq, tp::ErrorCode::InvalidParam); + return; + } + stream_config_ = requested; + polled_.store(want_polled); + streaming_.store(!want_polled); pty_.send_ack_ok(f.seq, f.cmd); return; + } case tp::Cmd::StopStream: + ++stream_stops_; + polled_.store(false); streaming_.store(false); pty_.send_ack_ok(f.seq, f.cmd); return; @@ -346,11 +453,100 @@ class FakeFollower { } } + void handle_exchange_(const xense::taccap::bus::Frame& f) { + tp::ExchangeRequestHeader h{}; + if (f.payload.size() < sizeof(h)) { pty_.send_nack(f.seq, tp::ErrorCode::LengthMismatch); return; } + std::memcpy(&h, f.payload.data(), sizeof(h)); + ++exchanges_; + const bool telemetry = h.flags & tp::ExchangeFlag::Telemetry; + if (telemetry) ++telemetry_exchanges_; + uint8_t result = tp::EXCHANGE_TARGET_RESULT_NONE; + if (h.target_kind != 0) { + ++exchange_targets_; + result = reject_next_.exchange(0); + if (h.target_kind == static_cast(tp::ExchangeTarget::Impedance) && + f.payload.size() == sizeof(h) + sizeof(tp::MotorImpedanceCtrl) && result == 0) { + std::lock_guard lk(mu_); + tp::MotorImpedanceCtrl c{}; + std::memcpy(&c, f.payload.data() + sizeof(h), sizeof(c)); + last_submit_ = c; + } + } + if (const int delay = exchange_delay_ms_.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(delay)); + if (auto extra = pty_.expect_frame(1)) { + ++overlaps_; + deferred_.push_back(*extra); + } + } + unsigned drop = drop_replies_.load(); + while (drop && !drop_replies_.compare_exchange_weak(drop, drop - 1)) {} + if (drop) return; + tp::ExchangeResponseHeader r{}; + r.version = tp::EXCHANGE_VERSION; + r.target_result = result; + std::vector out(sizeof(r)); + if (telemetry) { + std::lock_guard lk(mu_); + if (stream_config_.source_mask & tp::StreamSrc::MotorStatus) { + r.flags |= tp::ExchangeRespFlag::Motor; + auto s = pod_bytes(&status_, sizeof(status_)); + s.resize(tp::MOTOR_STATUS_V2_SIZE, 0); + out.insert(out.end(), s.begin(), s.end()); + } + if (stream_config_.source_mask & tp::StreamSrc::ImuV2) { + r.imu_cmd = static_cast(tp::Cmd::GetImuV2); + r.imu_count = 1; + auto imu = imu_v2_packet_(); + out.insert(out.end(), imu.begin(), imu.end()); + } else if (stream_config_.source_mask & tp::StreamSrc::Imu) { + r.imu_cmd = static_cast(tp::Cmd::GetImu); + r.imu_count = 1; + tp::ImuData imu{}; + imu.accel_z = 1000; + imu.seq = imu_seq_++; + auto b = pod_bytes(&imu, sizeof(imu)); + out.insert(out.end(), b.begin(), b.end()); + } + } + std::memcpy(out.data(), &r, sizeof(r)); + last_exchange_reply_ = out; + if (drop_after_build_) { --drop_after_build_; return; } // reply lost on the wire + pty_.send_response(f.seq, f.cmd, out); + auto later = std::move(deferred_); + deferred_.clear(); + for (auto& g : later) handle_(g); + } + + std::atomic exchange_supported_{true}, polled_{false}; + std::atomic exchanges_{0}, telemetry_exchanges_{0}, exchange_targets_{0}; + std::atomic overlaps_{0}, drop_replies_{0}, other_while_polled_{0}; + std::atomic drop_after_build_{0}, exchange_replays_{0}; + std::optional last_exchange_; + std::optional> last_exchange_reply_; + std::atomic exchange_delay_ms_{0}; + std::atomic reject_next_{0}; + std::vector deferred_; + Pty& pty_; uint8_t fw_major_, fw_minor_, fw_patch_; std::thread thread_; std::atomic stop_{false}; std::atomic streaming_{false}; + std::atomic imu_supported_{false}; + std::atomic imu_v2_supported_{false}; + uint64_t imu_session_{0}, imu_time_{UINT64_C(0x200000000)}; + uint16_t imu_seq_{0}; + std::vector imu_v2_packet_() { + imu_time_+=10000; + tp::ImuData raw{};raw.accel_z=1000;raw.timestamp_us=uint32_t(imu_time_);raw.seq=imu_seq_++; + auto p=pod_bytes(&raw,sizeof(raw)); + for(auto value:{imu_time_,imu_session_}) + for(unsigned i=0;i<8;i++)p.push_back(uint8_t(value>>(i*8))); + return p; + } + std::atomic imu_queries_{0}, stream_stops_{0}; + tp::StreamConfig stream_config_{}; std::atomic frozen_{false}; std::atomic submits_{0}; std::atomic disables_{0}; @@ -367,6 +563,7 @@ class FakeFollower { std::string sn_ = "TCGU01A28Z0001s"; mutable std::mutex mu_; tp::MotorStatus status_{}; + std::optional next_polled_status_word_; std::optional last_submit_; // Factory state: a valid travel calibration and a ZEROED envelope, which diff --git a/cpp/tests/fake_leader.hpp b/cpp/tests/fake_leader.hpp new file mode 100644 index 0000000..f7e4a7d --- /dev/null +++ b/cpp/tests/fake_leader.hpp @@ -0,0 +1,263 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// A minimal fake leader firmware over a pty for the leader exchange tests. +// It answers what LeaderGripper's constructor and start_streaming()/ +// start_exchange() issue, produces 100 Hz IMU (legacy 28 B) + encoder samples +// while streaming, and either pushes them as DATA frames (StreamMode +// Separate) or holds them as [cmd][len][payload] records for Cmd:: +// LeaderExchange (StreamMode::Polled), mirroring tc-gu-01 leader_uplink.c: +// 1 KB queue, oldest record dropped when full, 504 B of records per reply, +// and a byte-identical repeat of the last request replays the cached reply. + +#pragma once + +#include "pty_helper.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace taccap_test { + +class FakeLeader { +public: + explicit FakeLeader(Pty& pty) : pty_(pty) { thread_ = std::thread([this] { run_(); }); } + ~FakeLeader() { + stop_.store(true); + if (thread_.joinable()) thread_.join(); + } + + // Firmware without 0x5E: InvalidCmd for the exchange, InvalidParam for mode 2. + void set_exchange_supported(bool on) { exchange_supported_.store(on); } + // Produce samples every this many ms while streaming (default 10). + void set_sample_period_ms(int ms) { sample_ms_.store(ms); } + // Hold each exchange reply this long and count host frames that arrive meanwhile. + void set_exchange_reply_delay_ms(int ms) { delay_ms_.store(ms); } + // Do not answer the next n exchange requests (records are still taken). + void drop_exchange_replies(unsigned n) { drop_.store(n); } + // Make the next exchange reply's record lengths inconsistent. + void corrupt_next_reply() { corrupt_.store(true); } + // Firmware lease fallback (tc-gu-01 LEADER_EXCHANGE.md): leave the exchange, + // flush held records as DATA frames, then push as the original firmware. + void expire_lease() { expire_.store(true); } + // A button event, held or pushed like any other DATA frame. Emitted from + // the fake's own thread so the PTY never has two writers. + void press_key(uint8_t state) { key_state_.store(static_cast(state)); } + + bool polled() const { return polled_.load(); } + bool streaming() const { return streaming_.load(); } + uint8_t stream_mode() const { return mode_.load(); } + unsigned exchanges() const { return exchanges_.load(); } + unsigned lease_expiries() const { return lease_expiries_.load(); } + uint16_t stream_source_mask() const { return source_mask_.load(); } + unsigned time_session_requests() const { return time_sessions_.load(); } + unsigned polled_starts() const { return polled_starts_.load(); } + unsigned replays() const { return replays_.load(); } + unsigned overlaps() const { return overlaps_.load(); } + // DATA frames written on the wire (only outside StreamMode::Polled). + unsigned data_frames_pushed() const { return pushed_.load(); } + uint16_t imu_seq_produced() const { return imu_seq_.load(); } + +private: + struct Record { uint8_t cmd; std::vector payload; }; + + void emit_(tp::Cmd cmd, const void* p, std::size_t n) { + std::lock_guard lk(mu_); + std::vector bytes(static_cast(p), static_cast(p) + n); + if (polled_.load()) { + queue_bytes_ += 2 + n; + queue_.push_back({static_cast(cmd), std::move(bytes)}); + while (queue_bytes_ > 1024) { + queue_bytes_ -= 2 + queue_.front().payload.size(); + queue_.pop_front(); + if (dropped_ != 0xFFFF) ++dropped_; + } + } else { + pty_.send_data(0, cmd, bytes); + ++pushed_; + } + } + + void produce_() { + tp::ImuData imu{}; + imu.valid_flag = 0x0F; + imu.accel_z = 1000; + imu.seq = imu_seq_.load(); + imu.timestamp_us = 10000u * imu.seq + 1u; + emit_(tp::Cmd::GetImu, &imu, sizeof(imu)); + tp::EncoderData enc{}; + enc.seq = imu.seq; + enc.timestamp_us = imu.timestamp_us; + enc.position_rad = 0.01f * static_cast(imu.seq % 100); + emit_(tp::Cmd::GetEncoder, &enc, sizeof(enc)); + imu_seq_.fetch_add(1); + } + + std::vector take_reply_() { + std::lock_guard lk(mu_); + tp::LeaderExchangeResponseHeader h{}; + h.version = tp::LEADER_EXCHANGE_VERSION; + h.flags = polled_.load() ? tp::LeaderExchangeRespFlag::Polled : 0; + std::vector out(sizeof(h)); + while (!queue_.empty() && out.size() + 2 + queue_.front().payload.size() <= 512) { + auto& r = queue_.front(); + out.push_back(r.cmd); + out.push_back(static_cast(r.payload.size())); + out.insert(out.end(), r.payload.begin(), r.payload.end()); + queue_bytes_ -= 2 + r.payload.size(); + queue_.pop_front(); + ++h.record_count; + } + h.dropped = dropped_; + dropped_ = 0; + h.pending = static_cast(queue_.size()); + std::memcpy(out.data(), &h, sizeof(h)); + return out; + } + + void handle_(const tb::Frame& f) { + if (pending_reply_) ++overlaps_; + // Duplicate request (same seq/cmd/payload): replay the cached reply. + if (last_ && last_->seq == f.seq && last_->cmd == f.cmd && last_->payload == f.payload) { + ++replays_; + if (f.cmd == tp::Cmd::LeaderExchange) pty_.send_response(f.seq, f.cmd, last_reply_); + else pty_.send_ack_ok(f.seq, f.cmd); + return; + } + last_ = f; + switch (f.cmd) { + case tp::Cmd::StopStream: { + std::lock_guard lk(mu_); + streaming_.store(false); polled_.store(false); + queue_.clear(); queue_bytes_ = 0; dropped_ = 0; + pty_.send_ack_ok(f.seq, f.cmd); + return; + } + case tp::Cmd::StartStream: { + const auto sc = tp::decode_stream_config(f.payload.data(), f.payload.size()); + if (sc.mode == static_cast(tp::StreamMode::Polled) && !exchange_supported_.load()) { + pty_.send_nack(f.seq, tp::ErrorCode::InvalidParam); + return; + } + { + std::lock_guard lk(mu_); + queue_.clear(); queue_bytes_ = 0; dropped_ = 0; + polled_.store(sc.mode == static_cast(tp::StreamMode::Polled)); + if (polled_.load()) ++polled_starts_; + } + mode_.store(sc.mode); + source_mask_.store(sc.source_mask); + streaming_.store(true); + pty_.send_ack_ok(f.seq, f.cmd); + return; + } + case tp::Cmd::LeaderExchange: { + if (!exchange_supported_.load()) { pty_.send_nack(f.seq, tp::ErrorCode::InvalidCmd); return; } + if (f.payload.size() != sizeof(tp::LeaderExchangeRequest) || + f.payload[0] != tp::LEADER_EXCHANGE_VERSION || f.payload[1] || f.payload[2] || f.payload[3]) { + pty_.send_nack(f.seq, tp::ErrorCode::InvalidParam); + return; + } + ++exchanges_; + last_reply_ = take_reply_(); + if (corrupt_.exchange(false) && last_reply_.size() > 9) last_reply_[9] = 0xFF; + if (drop_.load()) { drop_.fetch_sub(1); return; } + if (const int ms = delay_ms_.load()) { + pending_reply_ = true; + const auto until = std::chrono::steady_clock::now() + std::chrono::milliseconds(ms); + while (std::chrono::steady_clock::now() < until) { + if (auto g = pty_.expect_frame(1)) { ++overlaps_; deferred_.push_back(*g); } + } + pending_reply_ = false; + } + pty_.send_response(f.seq, f.cmd, last_reply_); + return; + } + case tp::Cmd::GetVersion: { + tp::FirmwareVersion v{1, 2, 6, 0}; + std::vector d(sizeof(v)); + std::memcpy(d.data(), &v, sizeof(v)); + pty_.send_response(f.seq, f.cmd, d); + return; + } + case tp::Cmd::GetSn: { + const char sn[] = "TCGU01FAKELEADm"; + pty_.send_response(f.seq, f.cmd, std::vector(sn, sn + sizeof(sn))); + return; + } + case tp::Cmd::Heartbeat: + pty_.send_response(f.seq, f.cmd, {1, 0, 0, 0, 1, 2, 6, 0}); + return; + default: + // TimeSession and anything else: legacy firmware. + if (f.cmd == tp::Cmd::TimeSession) ++time_sessions_; + pty_.send_nack(f.seq, tp::ErrorCode::InvalidCmd); + return; + } + } + + void run_() { + auto next = std::chrono::steady_clock::now(); + while (!stop_.load()) { + if (!deferred_.empty()) { + auto f = deferred_.front(); + deferred_.erase(deferred_.begin()); + handle_(f); + } else if (auto f = pty_.expect_frame(1)) { + handle_(*f); + } + if (expire_.exchange(false) && polled_.load()) { + std::lock_guard lk(mu_); + polled_.store(false); + mode_.store(static_cast(tp::StreamMode::Separate)); + for (auto& rec : queue_) { pty_.send_data(0, static_cast(rec.cmd), rec.payload); ++pushed_; } + queue_.clear(); queue_bytes_ = 0; + ++lease_expiries_; + } + const int key = key_state_.exchange(-1); + if (key >= 0) { + tp::KeyStatusPayload k{0, static_cast(key)}; + emit_(tp::Cmd::KeyStatus, &k, sizeof(k)); + } + const auto now = std::chrono::steady_clock::now(); + if (streaming_.load() && now >= next) { + next = now + std::chrono::milliseconds(sample_ms_.load()); + produce_(); + } + } + } + + Pty& pty_; + std::thread thread_; + std::atomic stop_{false}; + std::atomic exchange_supported_{true}; + std::atomic sample_ms_{10}; + std::atomic delay_ms_{0}; + std::atomic drop_{0}; + std::atomic corrupt_{false}; + std::atomic streaming_{false}, polled_{false}; + std::atomic mode_{0}; + std::atomic exchanges_{0}, replays_{0}, overlaps_{0}, pushed_{0}; + std::atomic key_state_{-1}; + std::atomic expire_{false}; + std::atomic lease_expiries_{0}, polled_starts_{0}, time_sessions_{0}; + std::atomic source_mask_{0}; + std::atomic imu_seq_{0}; + bool pending_reply_ = false; + std::vector deferred_; + std::optional last_; + std::vector last_reply_; + std::mutex mu_; + std::deque queue_; + std::size_t queue_bytes_ = 0; + uint16_t dropped_ = 0; +}; + +} // namespace taccap_test diff --git a/cpp/tests/test_follower_exchange_pty.cpp b/cpp/tests/test_follower_exchange_pty.cpp new file mode 100644 index 0000000..38c081a --- /dev/null +++ b/cpp/tests/test_follower_exchange_pty.cpp @@ -0,0 +1,282 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// FollowerGripper::start_exchange() against the fake follower: polled mode, +// telemetry fan-out to the ordinary subscribers, targets riding on exchanges +// instead of no-ACK frames, strict one-request-at-a-time on the link (with +// concurrent user queries), timeout resend, rejection reporting, and a +// ForcePositionController running unmodified on top. + +#include "fake_follower.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace tx = xense::taccap; +namespace tp = xense::taccap::protocol; + +using taccap_test::FakeFollower; +using taccap_test::Pty; +using taccap_test::open_follower; +using taccap_test::wait_for; + +namespace { +constexpr auto kSecond = std::chrono::milliseconds(1000); +} + +TEST(FollowerExchangePty, PolledModeFeedsStatusAndImuSubscribersWithoutStreaming) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto g = open_follower(pty); + + std::atomic status{0}, imu{0}; + g->motor().on_status([&](const tx::MotorStatusSample&) { ++status; }); + g->imu().on_data([&](const tx::ImuSample&) { ++imu; }); + g->start_exchange(100, 100); + + EXPECT_TRUE(g->is_exchanging()); + EXPECT_TRUE(g->is_streaming()); + EXPECT_TRUE(fw.polled()); + EXPECT_EQ(fw.stream_config().mode, static_cast(tp::StreamMode::Polled)); + ASSERT_TRUE(wait_for([&] { return status.load() >= 20 && imu.load() >= 20; }, 2 * kSecond)) + << "status=" << status << " imu=" << imu; + const auto s = g->exchange_stats(); + EXPECT_GE(s.telemetry_exchanges, 20u); + EXPECT_EQ(s.timeouts, 0u); + EXPECT_EQ(s.malformed, 0u); + EXPECT_EQ(s.motor_samples, s.telemetry_exchanges); + + g->stop_streaming(); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_FALSE(fw.polled()); + const unsigned after = fw.exchanges(); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + EXPECT_EQ(fw.exchanges(), after) << "exchange loop kept running after stop_streaming"; +} + +TEST(FollowerExchangePty, SubmitRidesOnAnExchangeAndGoesOutPromptly) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + g->start_exchange(10); // slow telemetry: a target must not wait for the tick + + ASSERT_TRUE(wait_for([&] { return fw.telemetry_exchanges() >= 1; }, kSecond)); + const unsigned targets = fw.exchange_targets(); + const auto t0 = std::chrono::steady_clock::now(); + g->motor().submit_impedance(0.25f, 2.0f, 0.1f, 0.3f, 0.0f); + ASSERT_TRUE(wait_for([&] { return fw.exchange_targets() > targets; }, kSecond)); + const auto latency = std::chrono::steady_clock::now() - t0; + EXPECT_LT(latency, std::chrono::milliseconds(30)) << "target waited for a telemetry tick"; + EXPECT_EQ(fw.submit_count(), 0u) << "no-ACK 0x43 frame written during exchange"; + const auto last = fw.last_submit(); + ASSERT_TRUE(last.has_value()); + EXPECT_FLOAT_EQ(last->target_pos, 0.25f); + EXPECT_FLOAT_EQ(last->kp, 2.0f); + EXPECT_FLOAT_EQ(last->target_torque, 0.3f); + // The fake records the target before the host has processed its reply. + EXPECT_TRUE(wait_for([&] { return g->exchange_stats().last_target_result == 0; }, kSecond)); + + g->stop_streaming(); + g->motor().submit_impedance(0.1f, 0.0f, 0.0f, 0.0f, 0.0f); + EXPECT_TRUE(wait_for([&] { return fw.submit_count() == 1; }, kSecond)) + << "after stop the target must go out as the ordinary no-ACK frame"; +} + +TEST(FollowerExchangePty, NeverWritesWhileAReplyIsPendingEvenWithConcurrentQueries) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + fw.set_exchange_reply_delay_ms(3); + g->start_exchange(100); + + std::atomic stop{false}; + std::atomic heartbeats{0}, heartbeat_timeouts{0}; + // 1 kHz of user queries, as in the leader test: an unthrottled loop starves + // the exchange on the (unfair) half-duplex turn lock, a documented limit. + std::thread user([&] { + while (!stop.load()) { + try { g->transport().send_cmd(tp::Cmd::Heartbeat, {}, std::chrono::milliseconds(200)); ++heartbeats; } + catch (...) { ++heartbeat_timeouts; } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); + std::thread submitter([&] { + for (int i = 0; i < 200 && !stop.load(); ++i) { + g->motor().submit_impedance(0.01f * i, 0, 0, 0, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + }); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + stop.store(true); + submitter.join(); + user.join(); + + EXPECT_GT(fw.exchanges(), 15u); + EXPECT_GT(heartbeats.load(), 5u); + EXPECT_GT(fw.exchange_targets(), 5u); + // The host may only write over a pending reply after giving up on it. On + // an unloaded machine that never happens; a starved test runner can delay + // the fake past the timeout, and then each overlap must be a timeout. + const auto gave_up = g->exchange_stats().timeouts + heartbeat_timeouts.load(); + EXPECT_LE(fw.overlaps(), gave_up) << "host wrote a frame while an exchange reply was pending"; + g->stop_streaming(); +} + +TEST(FollowerExchangePty, TimedOutTargetIsResentUnlessSuperseded) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + g->start_exchange(20); + ASSERT_TRUE(wait_for([&] { return fw.telemetry_exchanges() >= 1; }, kSecond)); + + // Two lost requests exhaust the same-seq retry (open_follower: max_retries=1), + // so the exchange times out and the loop resends the target itself. + fw.drop_exchange_replies(2); + const unsigned targets = fw.exchange_targets(); + g->motor().submit_impedance(0.5f, 1.0f, 0.0f, 0.0f, 0.0f); + ASSERT_TRUE(wait_for([&] { return fw.exchange_targets() >= targets + 2; }, kSecond)) + << "dropped target was not resent"; + ASSERT_TRUE(wait_for([&] { return g->exchange_stats().targets_resent >= 1; }, kSecond)); + const auto s = g->exchange_stats(); + EXPECT_GE(s.timeouts, 1u); + ASSERT_TRUE(fw.last_submit().has_value()); + EXPECT_FLOAT_EQ(fw.last_submit()->target_pos, 0.5f); + g->stop_streaming(); +} + +TEST(FollowerExchangePty, LostReplyIsReplayedBySameSeqRetryNotLost) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto g = open_follower(pty); + std::mutex m; std::vector seqs; + g->imu().on_data([&](const tx::ImuSample& s) { std::lock_guard lk(m); seqs.push_back(s.seq); }); + g->start_exchange(100, 100, 50); + ASSERT_TRUE(wait_for([&] { std::lock_guard lk(m); return seqs.size() >= 10; }, kSecond)); + + const auto acks_before = g->transport().stats().ack_timeouts; + fw.lose_exchange_replies(1); // telemetry built (IMU consumed) but lost on the wire + ASSERT_TRUE(wait_for([&] { return fw.exchange_replays() >= 1; }, kSecond)) << "no same-seq retry"; + std::size_t at; + { std::lock_guard lk(m); at = seqs.size(); } + ASSERT_TRUE(wait_for([&] { std::lock_guard lk(m); return seqs.size() >= at + 20; }, kSecond)); + EXPECT_GT(g->transport().stats().ack_timeouts, acks_before); + EXPECT_EQ(g->exchange_stats().timeouts, 0u) << "the retry recovered the reply"; + std::lock_guard lk(m); + unsigned missing = 0; + for (std::size_t i = 1; i < seqs.size(); ++i) { + const uint16_t d = static_cast(seqs[i] - seqs[i - 1]); + if (d != 1) missing += d == 0 ? 1u : d - 1u; + } + EXPECT_EQ(missing, 0u) << "IMU samples of the lost reply were lost"; + g->stop_streaming(); +} + +TEST(FollowerExchangePty, RejectedTargetIsCountedAndTelemetryContinues) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + std::atomic status{0}; + g->motor().on_status([&](const tx::MotorStatusSample&) { ++status; }); + g->start_exchange(100); + + fw.reject_next_exchange_target(static_cast(tp::ErrorCode::SysBusy)); + g->motor().submit_impedance(0.2f, 0, 0, 0, 0); + ASSERT_TRUE(wait_for([&] { return g->exchange_stats().targets_rejected == 1; }, kSecond)); + EXPECT_EQ(g->exchange_stats().last_target_result, static_cast(tp::ErrorCode::SysBusy)); + const unsigned before = status.load(); + EXPECT_TRUE(wait_for([&] { return status.load() >= before + 5; }, kSecond)); + g->stop_streaming(); +} + +TEST(FollowerExchangePty, OldFirmwareNacksAndNothingIsLeftRunning) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_exchange_supported(false); + auto g = open_follower(pty); + EXPECT_THROW(g->start_exchange(100), tx::ProtocolError); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_FALSE(g->is_streaming()); + EXPECT_FALSE(g->transport().half_duplex()); + g->motor().submit_impedance(0.1f, 0, 0, 0, 0); + EXPECT_TRUE(wait_for([&] { return fw.submit_count() == 1; }, kSecond)); +} + +TEST(FollowerExchangePty, ArgumentsAndStateAreValidated) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + EXPECT_THROW(g->start_exchange(0, 0), tx::IoError); + EXPECT_THROW(g->start_exchange(101), tx::IoError); + EXPECT_THROW(g->start_exchange(100, 101), tx::IoError); + EXPECT_THROW(g->start_exchange(100, 0, 501), tx::IoError); + g->start_streaming(100); + EXPECT_THROW(g->start_exchange(100), tx::IoError); + g->stop_streaming(); + g->start_exchange(100); + EXPECT_TRUE(g->transport().half_duplex()); + g->close(); + EXPECT_FALSE(g->is_exchanging()); +} + +TEST(FollowerExchangePty, ForcePositionControllerRunsUnmodifiedOverTheExchange) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_exchange_reply_delay_ms(1); + auto g = open_follower(pty); + g->start_exchange(100); + + tx::ForcePositionConfig cfg; + tx::ForcePositionController c(*g, cfg); + c.start(); + ASSERT_TRUE(c.running()); + ASSERT_TRUE(wait_for([&] { return fw.exchange_targets() >= 20; }, 2 * kSecond)) + << "controller targets via exchange: " << fw.exchange_targets(); + EXPECT_EQ(fw.submit_count(), 0u); + EXPECT_EQ(c.state(), tx::ForcePositionState::HoldingPosition); + EXPECT_TRUE(c.snapshot().observation.valid); + EXPECT_LE(fw.overlaps(), g->exchange_stats().timeouts); + c.stop(); + EXPECT_TRUE(g->is_exchanging()) << "controller must not stop a stream it did not start"; + g->stop_streaming(); +} + +TEST(FollowerExchangePty, WaitingForTheLinkCountsAgainstTheRequestTimeout) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + g->transport().set_half_duplex(true); + fw.drop_exchange_replies(1); // the first request holds the turn for its full 600 ms + + const std::vector req{tp::EXCHANGE_VERSION, tp::ExchangeFlag::Telemetry, 0, 0}; + std::thread holder([&] { + EXPECT_THROW(g->transport().send_cmd(tp::Cmd::SlaveExchange, req, std::chrono::milliseconds(600), + tx::bus::RetryMode::Once), + tx::TimeoutError); + }); + ASSERT_TRUE(wait_for([&] { return fw.exchanges() >= 1; }, kSecond)); + const auto t0 = std::chrono::steady_clock::now(); + EXPECT_THROW(g->transport().send_cmd(tp::Cmd::Heartbeat, {}, std::chrono::milliseconds(50)), + tx::TimeoutError); + const auto waited = std::chrono::steady_clock::now() - t0; + EXPECT_LT(waited, std::chrono::milliseconds(300)) << "query waited behind a long request"; + holder.join(); + // Once the turn is free a query goes through normally. + EXPECT_NO_THROW(g->transport().send_cmd(tp::Cmd::Heartbeat, {}, std::chrono::milliseconds(200))); + g->transport().set_half_duplex(false); +} diff --git a/cpp/tests/test_follower_imu_pty.cpp b/cpp/tests/test_follower_imu_pty.cpp new file mode 100644 index 0000000..269241b --- /dev/null +++ b/cpp/tests/test_follower_imu_pty.cpp @@ -0,0 +1,92 @@ +#include "fake_follower.hpp" +#include +#include +#include +#include + +namespace tx = xense::taccap; +namespace tp = tx::protocol; +using namespace taccap_test; + +TEST(FollowerImu, DefaultRemainsMotorOnlyWithoutProbe) { + Pty pty; FakeFollower fw(pty); auto g = open_follower(pty); + g->start_streaming(); + EXPECT_EQ(fw.imu_queries(), 0u); + EXPECT_EQ(fw.stream_config().source_mask, tp::StreamSrc::MotorStatus); + EXPECT_EQ(fw.stream_config().imu_rate, 0u); + EXPECT_EQ(fw.stream_config().motor_rate, 100u); + EXPECT_THROW(g->start_streaming(100, 50), tx::IoError); +} + +TEST(FollowerImu, OldFirmwareFailsBeforeChangingStream) { + Pty pty; FakeFollower fw(pty); auto g = open_follower(pty); + const auto stops = fw.stream_stops(); + EXPECT_THROW(g->start_streaming(100, 50), tx::ProtocolError); + EXPECT_EQ(fw.stream_stops(), stops); + EXPECT_FALSE(g->is_streaming()); + EXPECT_EQ(fw.stream_config().source_mask, 0u); + EXPECT_NO_THROW(g->start_streaming(100)); +} + +TEST(FollowerImu, OptInDeliversBothCallbacksWithUnchangedMotorRate) { + Pty pty; FakeFollower fw(pty); fw.set_imu_supported(true); + auto g = open_follower(pty); + std::atomic imus{0}, motors{0}; + auto i = g->imu().on_data([&](const tx::ImuSample& s) { + if (std::abs(s.accel_mps2[2] - 9.80665f) < .001f) ++imus; + }); + auto m = g->motor().on_status([&](const auto&) { ++motors; }); + g->start_streaming(100, 50); + for (int n = 0; n < 100 && (!imus || !motors); ++n) + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + EXPECT_GT(imus.load(), 0u); EXPECT_GT(motors.load(), 0u); + EXPECT_EQ(fw.stream_config().motor_rate, 100u); + EXPECT_EQ(fw.stream_config().imu_rate, 50u); + EXPECT_EQ(fw.stream_config().source_mask, tp::StreamSrc::Imu | tp::StreamSrc::MotorStatus); + EXPECT_EQ(fw.stream_config().mode, static_cast(tp::StreamMode::Separate)); + g->stop_streaming(); + g->imu().off(i); g->motor().off(m); + g->close(); // Join dispatcher before callback captures go out of scope. +} + +TEST(FollowerImu, ImuOnlyAndInvalidRates) { + Pty pty; FakeFollower fw(pty); fw.set_imu_supported(true); + auto g = open_follower(pty); + EXPECT_THROW(g->start_streaming(0, 0), tx::IoError); + EXPECT_THROW(g->start_streaming(100, 101), tx::IoError); + EXPECT_EQ(fw.imu_queries(), 0u); + g->start_streaming(0, 25); + EXPECT_EQ(fw.stream_config().source_mask, tp::StreamSrc::Imu); + EXPECT_EQ(fw.stream_config().motor_rate, 0u); + EXPECT_THROW(g->start_streaming(), tx::IoError); +} + +TEST(FollowerImu, CalibrationSavesOnlyAfterSuccessfulVerification) { + Pty pty; tx::bus::Transport transport(base_config(pty.slave_path())); + tx::IMU imu(transport); + std::vector actions; + std::thread device([&] { + for (int n = 0; n < 2; ++n) { + auto f = pty.expect_frame(); + if (!f) return; + EXPECT_EQ(f->cmd, tp::Cmd::SetImuCal); + actions.push_back(f->payload.at(0)); + pty.send_ack_ok(f->seq, f->cmd); + } + }); + EXPECT_NO_THROW(imu.calibrate_accel_gyro()); + device.join(); + EXPECT_EQ(actions, (std::vector{1, 4})); +} + +TEST(FollowerImu, CalibrationNackDoesNotSave) { + Pty pty; tx::bus::Transport transport(base_config(pty.slave_path())); + tx::IMU imu(transport); + std::thread device([&] { + auto f = pty.expect_frame(); + if (f) pty.send_nack(f->seq, tp::ErrorCode::SysBusy); + EXPECT_FALSE(pty.expect_frame(50).has_value()); + }); + EXPECT_THROW(imu.calibrate_accel_gyro(), tx::ProtocolError); + device.join(); +} diff --git a/cpp/tests/test_frame.cpp b/cpp/tests/test_frame.cpp index 21c8304..8cda4bd 100644 --- a/cpp/tests/test_frame.cpp +++ b/cpp/tests/test_frame.cpp @@ -7,6 +7,7 @@ #include #include +#include namespace tb = xense::taccap::bus; namespace tp = xense::taccap::protocol; @@ -383,3 +384,229 @@ TEST(FrameParser, ResetKeepsDamageCounters) { EXPECT_EQ(p.buffered_bytes(), 0u); EXPECT_EQ(p.stats().crc_errors, 1u); } + +namespace { + +// Real follower bytes captured before SDK parsing on 2026-10-26, motor +// disabled; no pack_frame()/SDK encoder is used to construct these fixtures. +// Evidence: dist/debug/runs/imu-link-fix-504134a-20261026/logs/idle-uart.strace +// SHA-256: 9c939e9728d2e20b67a448536fb6db06cf89ec8a75d1cce627c96cb7c5010afb +// RX offsets below refer to the concatenated successful read() buffers. +// These bytes prove parser behavior, not firmware timing or loss origin. +std::vector captured_hex(const char* text) { + const std::string hex(text); + std::vector bytes; + for (std::size_t i = 0; i < hex.size(); i += 2) { + bytes.push_back(static_cast(std::stoul(hex.substr(i, 2), nullptr, 16))); + } + return bytes; +} + +struct CapturedFrame { + const char* name; + std::vector wire; + uint8_t seq; + tp::FrameType type; + tp::Cmd cmd; + std::vector payload; +}; + +std::vector captured_good_frames() { + return { + // Line 30, RX offset 307, 69 wire bytes; extended motor status. + {"motor DATA", captured_hex( + "aa025703503b00c05987bf001048bc0010c0ba000004420000262887bf" + "000000000000000004021301850000000000000000db587b00e85a7b00" + "00000000af2d78005b0a55"), + 0x57, tp::FrameType::DATA, tp::Cmd::GetMotorStatus, captured_hex( + "c05987bf001048bc0010c0ba000004420000262887bf0000000000000000" + "04021301850000000000000000db587b00e85a7b0000000000af2d7800")}, + // Line 7627, RX offset 245546, 55 wire bytes; payload contains 7d75. + {"IMU V2 DATA", captured_hex( + "aa022803172c00016a749d0f000300ffffe803f4ff1200faff7d75f65f" + "ecb211360e1e7c016a749d060000002bf06c202a29f306c7dd55"), + 0x28, tp::FrameType::DATA, tp::Cmd::GetImuV2, captured_hex( + "016a749d0f000300ffffe803f4ff1200faff55f65fecb211360e1e7c01" + "6a749d060000002bf06c202a29f306")}, + // Line 29, RX offset 248, 59 wire bytes; CRC contains escaped 0x55. + {"control stats ACK", captured_hex( + "aa020a025130000004f4010200ffff00000000344a0100324a0100fe71" + "0600000000002200000000000000000000000000000000000000637d7555"), + 0x0a, tp::FrameType::ACK, tp::Cmd::GetMotorControlStats, captured_hex( + "0004f4010200ffff00000000344a0100324a0100fe7106000000000022" + "00000000000000000000000000000000000000")}, + }; +} + +std::vector captured_short_motor_frame() { + // Lines 55 and 58 (unfinished/resumed read), RX offset 1138, 67 wire + // bytes. LEN is 59 but the unstuffed body is 65 bytes instead of 67. + // This is the captured damage, not a test-side deletion from a good frame. + return captured_hex( + "aa025e03503b00c05987bf001048bc0010c0ba000004420000262887bf" + "00000000000000000402130185000000000000db587b00245b7b000000" + "0000af2d78008f5155"); +} + +std::vector captured_stream(const std::vector& frames, + bool include_damage = false) { + std::vector stream; + for (std::size_t i = 0; i < frames.size(); ++i) { + stream.insert(stream.end(), frames[i].wire.begin(), frames[i].wire.end()); + if (include_damage && i == 0) { + const auto bad = captured_short_motor_frame(); + stream.insert(stream.end(), bad.begin(), bad.end()); + } + } + return stream; +} + +void expect_captured_frames(tb::FrameParser& parser, + const std::vector& expected) { + ASSERT_EQ(parser.pending(), expected.size()); + for (const auto& fixture : expected) { + SCOPED_TRACE(fixture.name); + tb::Frame frame; + ASSERT_TRUE(parser.try_pop(frame)); + expect_frame_eq(frame, tp::Address::MCU, fixture.seq, fixture.type, + fixture.cmd, fixture.payload); + } + tb::Frame extra; + EXPECT_FALSE(parser.try_pop(extra)); + EXPECT_EQ(parser.buffered_bytes(), 0u); + EXPECT_EQ(parser.stats().overflow_bytes, 0u); +} + +} // namespace + +TEST(HardwareFrameReplay, ValidFramesAtEverySingleSplit) { + for (const auto& fixture : captured_good_frames()) { + SCOPED_TRACE(fixture.name); + for (std::size_t split = 0; split <= fixture.wire.size(); ++split) { + SCOPED_TRACE(split); + tb::FrameParser parser; + parser.feed(fixture.wire.data(), split); + EXPECT_EQ(parser.pending(), split == fixture.wire.size() ? 1u : 0u); + parser.feed(fixture.wire.data() + split, fixture.wire.size() - split); + expect_captured_frames(parser, {fixture}); + EXPECT_EQ(parser.stats().resync_bytes, 0u); + EXPECT_EQ(parser.stats().crc_errors, 0u); + } + } +} + +TEST(HardwareFrameReplay, CoalescedValidFramesAtEverySingleSplit) { + const auto fixtures = captured_good_frames(); + const auto stream = captured_stream(fixtures); + // Includes one coalesced read and every boundary across DATA and ACK, + // including a split immediately after ESC in payload or CRC. + for (std::size_t split = 0; split <= stream.size(); ++split) { + SCOPED_TRACE(split); + tb::FrameParser parser; + parser.feed(stream.data(), split); + parser.feed(stream.data() + split, stream.size() - split); + expect_captured_frames(parser, fixtures); + EXPECT_EQ(parser.stats().resync_bytes, 0u); + EXPECT_EQ(parser.stats().crc_errors, 0u); + } +} + +TEST(HardwareFrameReplay, ValidFramesByteByByte) { + const auto fixtures = captured_good_frames(); + const auto stream = captured_stream(fixtures); + tb::FrameParser parser; + for (const auto byte : stream) parser.feed(&byte, 1); + expect_captured_frames(parser, fixtures); + EXPECT_EQ(parser.stats().resync_bytes, 0u); + EXPECT_EQ(parser.stats().crc_errors, 0u); +} + +TEST(HardwareFrameReplay, CapturedShortMotorFrameIsRejectedBeforeCrc) { + const auto bad = captured_short_motor_frame(); + ASSERT_EQ(bad.size(), 67u); + const auto result = tb::try_parse_frame(bad.data(), bad.size()); + EXPECT_EQ(result.status, tb::ParseStatus::Resync); + EXPECT_EQ(result.reason, tb::ParseReason::BadLength); + tb::FrameParser parser; + parser.feed(bad); + EXPECT_EQ(parser.pending(), 0u); + EXPECT_EQ(parser.buffered_bytes(), 0u); + EXPECT_EQ(parser.stats().resync_bytes, bad.size()); + EXPECT_EQ(parser.stats().crc_errors, 0u) + << "zero CRC errors cannot establish intact input when LEN already fails"; + EXPECT_EQ(parser.stats().bad_length, 1u); +} + +TEST(FrameTrace, DisabledByDefaultAndRawBytesPreservedAcrossReads) { + tb::FrameParser parser; + const auto bad = captured_short_motor_frame(); + parser.feed(bad); + EXPECT_TRUE(parser.take_trace().records.empty()); + parser.set_trace_enabled(true); + const auto good = captured_stream(captured_good_frames()); + parser.feed(good.data(), 13); + parser.feed(good.data() + 13, good.size() - 13); + parser.feed(bad); + const auto snapshot = parser.take_trace(); + std::vector input; + bool saw_length = false; + uint64_t offset = bad.size(); + for (const auto& r : snapshot.records) { + if (r.kind == tb::FrameParser::TraceRecord::Kind::Input) { + EXPECT_EQ(r.offset, offset); + offset += r.size; + input.insert(input.end(), r.bytes.begin(), r.bytes.begin() + r.size); + } else if (r.reason == tb::ParseReason::BadLength) { + saw_length = true; + EXPECT_EQ(r.offset, bad.size() + good.size()); + EXPECT_EQ(r.available, bad.size()); + EXPECT_EQ(r.bytes[0], 0xAA); + } + } + auto expected = good; + expected.insert(expected.end(), bad.begin(), bad.end()); + EXPECT_EQ(input, expected); + EXPECT_TRUE(saw_length); + EXPECT_EQ(snapshot.overwritten_records, 0u); + EXPECT_TRUE(parser.take_trace().records.empty()); +} + +TEST(FrameTrace, BoundedRingReportsOverwriteAndDisableClearsEvidence) { + tb::FrameParser parser; + parser.set_trace_enabled(true); + const auto frame = captured_good_frames().front().wire; + for (unsigned i = 0; i < 300; ++i) parser.feed(frame); + const auto snapshot = parser.take_trace(); + ASSERT_EQ(snapshot.records.size(), 256u); + EXPECT_EQ(snapshot.overwritten_records, 44u); + EXPECT_EQ(snapshot.records.front().offset, 44 * frame.size()); + EXPECT_EQ(snapshot.records.back().offset, 299 * frame.size()); + EXPECT_EQ(parser.pending(), 300u); + parser.set_trace_enabled(false); + parser.feed(frame); + EXPECT_TRUE(parser.take_trace().records.empty()); +} + +TEST(HardwareFrameReplay, RecoversAfterCapturedDamageAtEverySingleSplit) { + const auto fixtures = captured_good_frames(); + const auto stream = captured_stream(fixtures, true); + for (std::size_t split = 0; split <= stream.size(); ++split) { + SCOPED_TRACE(split); + tb::FrameParser parser; + parser.feed(stream.data(), split); + parser.feed(stream.data() + split, stream.size() - split); + expect_captured_frames(parser, fixtures); + EXPECT_EQ(parser.stats().resync_bytes, captured_short_motor_frame().size()); + EXPECT_EQ(parser.stats().crc_errors, 0u); + } +} + +TEST(HardwareFrameReplay, RecoversAfterCapturedDamageByteByByte) { + const auto fixtures = captured_good_frames(); + const auto stream = captured_stream(fixtures, true); + tb::FrameParser parser; + for (const auto byte : stream) parser.feed(&byte, 1); + expect_captured_frames(parser, fixtures); + EXPECT_EQ(parser.stats().resync_bytes, captured_short_motor_frame().size()); + EXPECT_EQ(parser.stats().crc_errors, 0u); +} diff --git a/cpp/tests/test_impedance_pty.cpp b/cpp/tests/test_impedance_pty.cpp index e9788c4..a7c0f4c 100644 --- a/cpp/tests/test_impedance_pty.cpp +++ b/cpp/tests/test_impedance_pty.cpp @@ -85,6 +85,13 @@ TEST(ImpedanceControllerPty, StaleStreamFaultsWithZeroTorqueAndAnInvalidObservat << "frozen numbers still presented as a live reading"; EXPECT_FALSE(snap.fault_reason.empty()); + // Fault is published before submit(), and the PTY peer consumes the frame + // on another thread. Wait for the safety action itself, not only state(). + ASSERT_TRUE(wait_for([&] { + const auto received = fw.last_submit(); + return received && received->kp == 0.0f && received->kd == 0.0f && + received->target_torque == 0.0f; + }, Ms(1000))); const auto s = fw.last_submit(); ASSERT_TRUE(s.has_value()); EXPECT_FLOAT_EQ(s->kp, 0.0f); diff --git a/cpp/tests/test_imu_motor_concurrency.cpp b/cpp/tests/test_imu_motor_concurrency.cpp new file mode 100644 index 0000000..95b810c --- /dev/null +++ b/cpp/tests/test_imu_motor_concurrency.cpp @@ -0,0 +1,461 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// User IMU delivery must not block control feedback or hide its receive age. +// Exercise that boundary through a PTY: +// the fake models packet flow, not firmware scheduling or physical motion. + +#include "fake_follower.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace tx = xense::taccap; +namespace tp = tx::protocol; +using taccap_test::FakeFollower; +using taccap_test::Pty; +using taccap_test::open_follower; +using taccap_test::wait_for; + +namespace { +using Ms = std::chrono::milliseconds; + +template struct ControllerTraits; +template <> struct ControllerTraits { + using Config = tx::ForcePositionConfig; + static constexpr auto fault = tx::ForcePositionState::Fault; +}; +template <> struct ControllerTraits { + using Config = tx::ImpedanceConfig; + static constexpr auto fault = tx::ImpedanceState::Fault; +}; + +// A deterministic slow subscriber: hold one callback until the test has +// observed the independent safety/close path, without relying on a particular +// scheduler delay. The release guard also unblocks early ASSERT returns. +class ImuCallbackGate { +public: + void operator()() { + ++calls; + std::unique_lock lock(mu_); + if (!armed_ || released_) return; + entered_ = true; + cv_.notify_all(); + cv_.wait(lock, [&] { return released_; }); + completed.store(true); + } + + void arm() { + std::lock_guard lock(mu_); + armed_ = true; + } + bool wait_until_entered() { + std::unique_lock lock(mu_); + return cv_.wait_for(lock, Ms(2000), [&] { return entered_; }); + } + void release() { + { + std::lock_guard lock(mu_); + released_ = true; + } + cv_.notify_all(); + } + + std::atomic calls{0}; + std::atomic completed{false}; + +private: + std::mutex mu_; + std::condition_variable cv_; + bool armed_ = false; + bool entered_ = false; + bool released_ = false; +}; + +struct ReleaseCallbackOnExit { + std::shared_ptr gate; + ~ReleaseCallbackOnExit() { gate->release(); } +}; + +bool is_zero_torque(const std::optional& command) { + return command && command->kp == 0.0f && command->kd == 0.0f && + command->target_torque == 0.0f && command->vel == 0.0f; +} + +template +void controller_keeps_preexisting_imu_stream() { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); // advancing timestamps and sample IDs + auto imus = std::make_shared>(0); + auto g = open_follower(pty); + const auto imu_sub = g->imu().on_data([imus](const tx::ImuSample&) { + ++*imus; + }); + g->start_streaming(100, 100); + const auto initial_stops = fw.stream_stops(); + + Controller controller(*g); + controller.start(); + ASSERT_TRUE(wait_for([&] { + return fw.submit_count() >= 8 && imus->load() >= 8; + }, Ms(2000))); + + // Repeated target updates must keep producing commands and IMU samples. + // Hold the fake's observed position: it deliberately has no motor model. + for (unsigned phase = 0; phase < 3; ++phase) { + const auto submits_before = fw.submit_count(); + const auto imus_before = imus->load(); + controller.set_target(controller.snapshot().observation.position); + ASSERT_TRUE(wait_for([&] { + return fw.submit_count() >= submits_before + 8 && + imus->load() >= imus_before + 8; + }, Ms(2000))); + EXPECT_TRUE(controller.snapshot().observation.valid); + EXPECT_NE(controller.state(), ControllerTraits::fault); + } + + const auto stream = fw.stream_config(); + EXPECT_EQ(stream.source_mask, + tp::StreamSrc::MotorStatus | tp::StreamSrc::ImuV2); + EXPECT_EQ(stream.motor_rate, 100u); + EXPECT_EQ(stream.imu_rate, 100u); + EXPECT_EQ(stream.mode, static_cast(tp::StreamMode::Separate)); + EXPECT_EQ(fw.stream_stops(), initial_stops); + + controller.stop(); + EXPECT_FALSE(controller.running()); + EXPECT_TRUE(g->is_streaming()); + EXPECT_EQ(fw.stream_stops(), initial_stops) + << "stopping a borrower must preserve the caller's shared stream"; + EXPECT_EQ(fw.disable_count(), 1u); + EXPECT_TRUE(is_zero_torque(fw.last_submit())); + + const auto stopped_submits = fw.submit_count(); + const auto imus_before = imus->load(); + ASSERT_TRUE(wait_for([&] { return imus->load() >= imus_before + 8; }, + Ms(2000))); + EXPECT_EQ(fw.submit_count(), stopped_submits); + const auto stats = g->transport().stats(); + EXPECT_EQ(stats.ack_timeouts, 0u); + EXPECT_EQ(stats.crc_errors, 0u); + EXPECT_EQ(stats.callback_exceptions, 0u); + EXPECT_EQ(stats.queue_dropped, 0u); + g->imu().off(imu_sub); + g->close(); +} + +template +void slow_imu_callback_isolated_and_real_loss_times_out() { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto gate = std::make_shared(); + auto g = open_follower(pty); + const auto imu_sub = g->imu().on_data([gate](const tx::ImuSample&) { + (*gate)(); + }); + g->start_streaming(100, 100); + typename ControllerTraits::Config config; + config.status_timeout_ms = 120; + Controller controller(*g, config); + ReleaseCallbackOnExit release_on_exit{gate}; + controller.start(); + ASSERT_TRUE(wait_for([&] { + return fw.submit_count() >= 4 && gate->calls.load() >= 4; + }, Ms(2000))); + ASSERT_TRUE(controller.snapshot().observation.valid); + + gate->arm(); + ASSERT_TRUE(gate->wait_until_entered()); + const auto submits_before = fw.submit_count(); + const auto frames_before = g->transport().stats().frames_received; + + // A blocked user callback must not stall control feedback. Keep it blocked + // for several status timeouts, while the independent control lane advances. + EXPECT_NO_THROW((void)g->device().heartbeat(Ms(500))); + ASSERT_TRUE(wait_for([&] { return fw.submit_count() >= submits_before + 30; }, Ms(2000))); + EXPECT_NE(controller.state(), ControllerTraits::fault); + EXPECT_TRUE(controller.snapshot().observation.valid); + EXPECT_FALSE(gate->completed.load()); + + // Real loss, not a user callback, must still trip the existing failsafe. + // Backlogged user DATA must never refresh the controller on release. + fw.freeze_stream(true); + ASSERT_TRUE(wait_for([&] { + return controller.state() == ControllerTraits::fault && + fw.submit_count() > submits_before && + is_zero_torque(fw.last_submit()); + }, Ms(2000))); + const auto snapshot = controller.snapshot(); + EXPECT_FALSE(snapshot.observation.valid); + EXPECT_EQ(snapshot.fault_reason, "motor status stream stale"); + EXPECT_FALSE(gate->completed.load()); + EXPECT_GT(g->transport().stats().frames_received, frames_before); + EXPECT_EQ(g->transport().stats().ack_timeouts, 0u); + + // Queued frames can resume after the callback returns; the fault must + // remain latched rather than silently resuming an old motion command. + const auto imus_before = gate->calls.load(); + gate->release(); + ASSERT_TRUE(wait_for([&] { + return gate->completed.load() && gate->calls.load() >= imus_before + 8; + }, Ms(2000))); + EXPECT_EQ(controller.state(), ControllerTraits::fault); + EXPECT_TRUE(is_zero_torque(fw.last_submit())); + controller.stop(); + EXPECT_EQ(fw.disable_count(), 1u); + g->imu().off(imu_sub); + g->close(); +} +} // namespace + +TEST(ImuMotorConcurrency, ForcePositionKeepsPreexistingCombinedStream) { + controller_keeps_preexisting_imu_stream(); +} + +TEST(ImuMotorConcurrency, ImpedanceKeepsPreexistingCombinedStream) { + controller_keeps_preexisting_imu_stream(); +} + +TEST(ImuMotorConcurrency, ForcePositionSlowImuIsIsolatedAndRealLossFaults) { + slow_imu_callback_isolated_and_real_loss_times_out(); +} + +TEST(ImuMotorConcurrency, ImpedanceSlowImuIsIsolatedAndRealLossFaults) { + slow_imu_callback_isolated_and_real_loss_times_out(); +} + +TEST(ImuMotorConcurrency, CloseJoinsActiveImuCallbackAndReleasesCaptures) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto gate = std::make_shared(); + auto capture = std::make_shared(42); + std::weak_ptr weak_capture = capture; + auto g = open_follower(pty); + ReleaseCallbackOnExit release_on_exit{gate}; + g->imu().on_data([gate, capture](const tx::ImuSample&) { (*gate)(); }); + capture.reset(); // only the registered callback now owns the resource + gate->arm(); + g->start_streaming(100, 100); + ASSERT_TRUE(gate->wait_until_entered()); + const auto frames_before = g->transport().stats().frames_received; + ASSERT_TRUE(wait_for([&] { + return g->transport().stats().frames_received >= frames_before + 8; + }, Ms(2000))); + + std::atomic close_returned{false}; + std::thread closer([&] { + g->close(); + close_returned.store(true); + }); + // Do not ASSERT while closer is joinable: release and join even when a + // check fails, so the test itself cannot strand the dispatcher. + EXPECT_TRUE(wait_for([&] { return !g->transport().is_running(); }, Ms(2000))); + EXPECT_FALSE(close_returned.load()); + EXPECT_FALSE(gate->completed.load()); + EXPECT_FALSE(weak_capture.expired()); + const auto calls_before_release = gate->calls.load(); + gate->release(); + closer.join(); + + EXPECT_TRUE(close_returned.load()); + EXPECT_TRUE(gate->completed.load()); + EXPECT_TRUE(g->is_closed()); + EXPECT_TRUE(weak_capture.expired()); + EXPECT_EQ(gate->calls.load(), calls_before_release) + << "close must discard queued DATA instead of entering more callbacks"; + EXPECT_GT(g->transport().stats().queue_dropped, 0u); +} + +TEST(ImuMotorConcurrency, QueuedUserSamplesKeepReceiveTime) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto gate = std::make_shared(); + auto g = open_follower(pty); + ReleaseCallbackOnExit release_on_exit{gate}; + std::atomic released{false}; + std::atomic old_motor{0}, old_imu{0}; + const auto motor_sub = g->motor().on_status([&](const tx::MotorStatusSample& s) { + if (released && std::chrono::steady_clock::now() - s.host_time >= Ms(80)) ++old_motor; + }); + const auto imu_sub = g->imu().on_data([&](const tx::ImuSample& s) { + (*gate)(); + if (released && std::chrono::steady_clock::now() - s.host_time >= Ms(80)) ++old_imu; + }); + gate->arm(); + g->start_streaming(100, 100); + ASSERT_TRUE(gate->wait_until_entered()); + auto before = g->transport().stats().frames_received; + ASSERT_TRUE(wait_for([&] { return g->transport().stats().frames_received >= before + 20; }, Ms(1000))); + fw.freeze_stream(true); + std::this_thread::sleep_for(Ms(120)); + released = true; + gate->release(); + EXPECT_TRUE(wait_for([&] { return old_motor >= 5 && old_imu >= 5; }, Ms(1000))); + g->motor().off(motor_sub); + g->imu().off(imu_sub); + g->close(); +} + +template +void sustained_slow_user_callbacks_do_not_retime_control() { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + fw.set_imu_v2_supported(true); + auto g = open_follower(pty); + const auto imu = g->imu().on_data([](const tx::ImuSample&) { std::this_thread::sleep_for(Ms(20)); }); + const auto motor = g->motor().on_status([](const tx::MotorStatusSample&) { std::this_thread::sleep_for(Ms(5)); }); + g->start_streaming(100, 100); + typename ControllerTraits::Config config; + config.status_timeout_ms = 150; + Controller controller(*g, config); + controller.start(); + const auto before = fw.submit_count(); + ASSERT_TRUE(wait_for([&] { return fw.submit_count() >= before + 60; }, Ms(1200))); + EXPECT_NE(controller.state(), ControllerTraits::fault); + EXPECT_TRUE(controller.snapshot().observation.valid); + EXPECT_EQ(g->transport().stats().control_queue_overflows, 0u); + fw.freeze_stream(true); + // State changes before submit() and the PTY peer consumes it. Observe both + // within the original deadline instead of racing the asynchronous send. + EXPECT_TRUE(wait_for([&] { + return controller.state() == ControllerTraits::fault && + is_zero_torque(fw.last_submit()); + }, Ms(500))); + controller.stop(); + g->motor().off(motor); + g->imu().off(imu); + g->close(); +} + +TEST(ImuMotorConcurrency, ForcePositionSustainedSlowCallbacksDoNotHideFeedbackLoss) { + sustained_slow_user_callbacks_do_not_retime_control(); +} + +TEST(ImuMotorConcurrency, ImpedanceSustainedSlowCallbacksDoNotHideFeedbackLoss) { + sustained_slow_user_callbacks_do_not_retime_control(); +} + +TEST(ImuMotorConcurrency, ControlQueueOverflowLatchesClosedAndRequiresReopen) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + tx::ForcePositionConfig config; + config.status_timeout_ms = 200; + tx::ForcePositionController controller(*g, config); + controller.start(); + ASSERT_TRUE(wait_for([&] { return fw.submit_count() >= 4; }, Ms(1000))); + fw.freeze_stream(true); + // A deliberately faster-than-real-wire burst exercises overload handling, + // not physical throughput. Each write stays below the PTY buffer size. + tp::MotorStatus status{}; + status.actual_pos = 0.60f; + const auto payload = taccap_test::pod_bytes(&status, sizeof(status)); + std::vector burst; + for (unsigned i = 0; i < 64; ++i) { + const auto frame = tx::bus::pack_frame(tp::Address::MCU, uint8_t(i), tp::FrameType::DATA, + tp::Cmd::GetMotorStatus, payload); + burst.insert(burst.end(), frame.begin(), frame.end()); + } + for (unsigned i = 0; i < 40 && !g->transport().stats().control_queue_overflows; ++i) { + pty.send_raw(burst); + std::this_thread::sleep_for(Ms(5)); + } + ASSERT_GT(g->transport().stats().control_queue_overflows, 0u); + EXPECT_GT(g->transport().stats().control_queue_dropped, 0u); + EXPECT_LE(g->transport().stats().control_queue_high_water, 8u); + fw.freeze_stream(false); + ASSERT_TRUE(wait_for([&] { + return controller.state() == tx::ForcePositionState::Fault && + is_zero_torque(fw.last_submit()); + }, Ms(1000))); + controller.stop(); + const auto stopped_submits = fw.submit_count(); + EXPECT_THROW(controller.start(), tx::IoError); + EXPECT_FALSE(controller.running()); + EXPECT_FALSE(controller.snapshot().observation.valid); + EXPECT_EQ(fw.submit_count(), stopped_submits); + g->close(); +} + +template +void fault_then_healthy_cannot_hide_safety_evidence() { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + Controller controller(*g); + controller.start(); + ASSERT_TRUE(wait_for([&] { return fw.submit_count() >= 4; }, Ms(1000))); + fw.freeze_stream(true); + tp::MotorStatus healthy{}; + healthy.actual_pos = 0.60f; + for (unsigned kind = 0; kind < 3; ++kind) { + auto bad = healthy; + if (kind == 0) bad.status = tp::MotorStatusBit::OverCurrent; + if (kind == 1) bad.actual_vel = std::numeric_limits::quiet_NaN(); + if (kind == 2) bad.actual_torque = 19.0f; + auto burst = tx::bus::pack_frame(tp::Address::MCU, 1, tp::FrameType::DATA, + tp::Cmd::GetMotorStatus, taccap_test::pod_bytes(&bad, sizeof(bad))); + const auto good = tx::bus::pack_frame(tp::Address::MCU, 2, tp::FrameType::DATA, + tp::Cmd::GetMotorStatus, taccap_test::pod_bytes(&healthy, sizeof(healthy))); + burst.insert(burst.end(), good.begin(), good.end()); + pty.send_raw(burst); + ASSERT_TRUE(wait_for([&] { return controller.state() == ControllerTraits::fault; }, Ms(1000))); + const auto reason = controller.snapshot().fault_reason; + EXPECT_NE(reason.find(kind == 0 ? "reports a fault" : kind == 1 ? "non-finite" : "torque exceeded"), std::string::npos); + EXPECT_TRUE(wait_for([&] { return is_zero_torque(fw.last_submit()); }, Ms(1000))); + EXPECT_EQ(g->transport().stats().control_queue_overflows, 0u); + controller.reset(); // explicit reset establishes a new receive epoch + } + controller.stop(); + g->close(); +} + +TEST(ImuMotorConcurrency, ForcePositionFaultThenHealthyBurstStaysFaulted) { + fault_then_healthy_cannot_hide_safety_evidence(); +} + +TEST(ImuMotorConcurrency, ImpedanceFaultThenHealthyBurstStaysFaulted) { + fault_then_healthy_cannot_hide_safety_evidence(); +} + +template +void initial_fault_ack_is_not_overwritten_by_healthy_stream() { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeFollower fw(pty); + auto g = open_follower(pty); + fw.set_next_polled_status_word(tp::MotorStatusBit::OverCurrent); + Controller controller(*g); + controller.start(); + EXPECT_TRUE(wait_for([&] { return controller.state() == ControllerTraits::fault; }, Ms(1000))); + EXPECT_EQ(controller.snapshot().fault_reason, "motor status reports a fault"); + EXPECT_TRUE(wait_for([&] { return is_zero_torque(fw.last_submit()); }, Ms(1000))); + controller.stop(); + g->close(); +} + +TEST(ImuMotorConcurrency, ForcePositionInitialFaultAckIsLatched) { + initial_fault_ack_is_not_overwritten_by_healthy_stream(); +} + +TEST(ImuMotorConcurrency, ImpedanceInitialFaultAckIsLatched) { + initial_fault_ack_is_not_overwritten_by_healthy_stream(); +} diff --git a/cpp/tests/test_imu_time.cpp b/cpp/tests/test_imu_time.cpp new file mode 100644 index 0000000..22db814 --- /dev/null +++ b/cpp/tests/test_imu_time.cpp @@ -0,0 +1,172 @@ +#include "fake_follower.hpp" +#include +#include +#include +#include + +namespace tx = xense::taccap; +namespace tp = tx::protocol; +using namespace taccap_test; + +static void append64(std::vector& p, uint64_t v) { + for(unsigned i=0;i<8;i++) p.push_back(uint8_t(v>>(i*8))); +} +static std::vector packet(uint64_t time, uint16_t seq, uint64_t session=0) { + tp::ImuData raw{}; raw.timestamp_us=uint32_t(time); raw.seq=seq; raw.accel_z=1000; + auto p=pod_bytes(&raw,sizeof(raw)); + if(session) {append64(p,time);append64(p,session);} + return p; +} +static tx::ImuSample sample(tx::IMU::Timeline& timeline,uint64_t time,uint16_t seq,uint64_t session=0) { + auto p=packet(time,seq,session); return timeline.decode(p.data(),p.size()); +} +TEST(ImuTime, NativeAcrossMultipleWrapsAndNewBoot) { + tx::IMU::Timeline t; + EXPECT_EQ(sample(t,17179869,1,123).mcu_timestamp_us,17179869u); + auto a=sample(t,(uint64_t(5)<<32)+10,2,123); + EXPECT_EQ(a.mcu_timestamp_us,(uint64_t(5)<<32)+10); + EXPECT_TRUE(a.timestamp_valid); EXPECT_FALSE(a.timestamp_discontinuity); + auto b=sample(t,1000,0,456); + EXPECT_EQ(b.boot_session_id,456u); EXPECT_EQ(b.mcu_timestamp_us,1000u); + EXPECT_TRUE(b.timestamp_valid); EXPECT_TRUE(b.timestamp_discontinuity); + EXPECT_FALSE(sample(t,2000,1,456).timestamp_discontinuity); +} +TEST(ImuTime, NativeReorderedFrameCannotMoveHighWaterMark) { + tx::IMU::Timeline t; sample(t,20000,2,123); + EXPECT_FALSE(sample(t,10000,1,123).timestamp_valid); + EXPECT_FALSE(sample(t,15000,1,123).timestamp_valid); + EXPECT_TRUE(sample(t,30000,3,123).timestamp_valid); +} +TEST(ImuTime, LegacyWrapAndSequenceWrap) { + tx::IMU::Timeline t; sample(t,0xfffffff0U,65535); + auto s=sample(t,0x10,0); + EXPECT_EQ(s.mcu_timestamp_us,UINT64_C(0x100000010)); + EXPECT_TRUE(s.timestamp_valid); EXPECT_FALSE(s.timestamp_is_64bit); + EXPECT_EQ(s.boot_session_id,0u); +} +TEST(ImuTime, LegacyDwtRegressionIsNotInventedAsWrap) { + tx::IMU::Timeline t; sample(t,17173802,15421); + auto s=sample(t,3933,15422); + EXPECT_EQ(s.mcu_timestamp_us,3933u); + EXPECT_FALSE(s.timestamp_valid); EXPECT_TRUE(s.timestamp_discontinuity); +} +TEST(ImuTime, LegacyRestartAndOutOfOrderAreFlagged) { + tx::IMU::Timeline t; sample(t,500000,200); + EXPECT_FALSE(sample(t,100,0).timestamp_valid); + sample(t,300,2); EXPECT_FALSE(sample(t,200,1).timestamp_valid); +} +TEST(ImuTime, LegacyRegressionRemainsInvalidAcrossDuplicatesAndForwardSamples) { + tx::IMU::Timeline t; + EXPECT_TRUE(sample(t,17173017,5113).timestamp_valid); + for (int i=0;i<10;++i) { + auto s=sample(t,3148,5114); + EXPECT_FALSE(s.timestamp_valid); + EXPECT_TRUE(s.timestamp_discontinuity); + } + EXPECT_FALSE(sample(t,13148,5115).timestamp_valid); + // Explicit new tracker establishes a new, unrelated legacy segment. + tx::IMU::Timeline restarted; + EXPECT_TRUE(sample(restarted,13148,5115).timestamp_valid); + // Native time carries a boot identity, so it can safely replace legacy. + auto native=sample(t,23148,5116,123); + EXPECT_TRUE(native.timestamp_valid); + EXPECT_TRUE(native.timestamp_discontinuity); +} +TEST(ImuTime, LegacyLongSilenceIsAmbiguousRatherThanInventedUptime) { + tx::IMU::Timeline t; + auto p=packet(100,1); auto start=std::chrono::steady_clock::now(); + t.decode(p.data(),p.size(),start); + p=packet(200,2); + auto s=t.decode(p.data(),p.size(),start+std::chrono::minutes(80)); + EXPECT_FALSE(s.timestamp_valid);EXPECT_TRUE(s.timestamp_discontinuity); +} +TEST(ImuTime, RejectsCorruptNativeTimestampAndSession) { + auto p=packet(UINT64_C(0x100000001),2,123); + p[0]=2; EXPECT_THROW(tx::IMU::decode(p.data(),p.size()),tx::ProtocolError); + p[0]=1; std::fill(p.begin()+36,p.end(),0); + EXPECT_THROW(tx::IMU::decode(p.data(),p.size()),tx::ProtocolError); + p.resize(43); EXPECT_THROW(tx::IMU::decode(p.data(),p.size()),tx::ProtocolError); +} +TEST(ImuTimePty, NegotiatesAndReadsNativeFrame) { + Pty pty; tx::bus::Transport transport(base_config(pty.slave_path())); tx::IMU imu(transport); + std::thread device([&] { + auto f=pty.expect_frame(); ASSERT_TRUE(f); EXPECT_EQ(f->cmd,tp::Cmd::TimeSession); + EXPECT_EQ(f->payload.size(),8u); + std::vector reply; append64(reply,UINT64_C(0x200000000));append64(reply,123); + pty.send_response(f->seq,f->cmd,reply); + f=pty.expect_frame(); ASSERT_TRUE(f); EXPECT_EQ(f->cmd,tp::Cmd::GetImuV2); + pty.send_response(f->seq,f->cmd,packet(UINT64_C(0x200000010),3,123)); + }); + auto s=imu.read_once(std::chrono::milliseconds{500}); + EXPECT_EQ(s.mcu_timestamp_us,UINT64_C(0x200000010)); EXPECT_EQ(s.boot_session_id,123u); + device.join(); +} +TEST(ImuTimePty, OnlyUnsupportedCommandPermitsFallback) { + for(auto error : {tp::ErrorCode::InvalidCmd,tp::ErrorCode::SysBusy}) { + Pty pty; tx::bus::Transport transport(base_config(pty.slave_path())); tx::IMU imu(transport); + std::thread device([&] {auto f=pty.expect_frame(); ASSERT_TRUE(f); pty.send_nack(f->seq,error);}); + if(error==tp::ErrorCode::InvalidCmd) EXPECT_EQ(imu.negotiate_time(),tp::StreamSrc::Imu); + else EXPECT_THROW(imu.negotiate_time(),tx::NackError); + device.join(); + } +} +TEST(ImuTimePty, TimeoutDoesNotSilentlyDowngrade) { + Pty pty; auto config=base_config(pty.slave_path()); config.max_retries=0; + tx::bus::Transport transport(config); tx::IMU imu(transport); + EXPECT_THROW(imu.negotiate_time(std::chrono::milliseconds{20}),tx::TimeoutError); +} +TEST(ImuTimePty, MalformedCapabilityResponseDoesNotDowngrade) { + Pty pty; tx::bus::Transport transport(base_config(pty.slave_path())); tx::IMU imu(transport); + std::thread device([&] {auto f=pty.expect_frame(); ASSERT_TRUE(f); + pty.send_response(f->seq,f->cmd,{0,0,0});}); + EXPECT_THROW(imu.negotiate_time(),tx::ProtocolError); + device.join(); +} +TEST(ImuTimePty, NativeStreamFansOutAndOffRemovesBothFormats) { + Pty pty; FakeFollower fw(pty); fw.set_imu_v2_supported(true); + auto g=open_follower(pty); + std::atomic a{0}, b{0}, motors{0}; + auto first=g->imu().on_data([&](const auto& s) { + if(s.timestamp_is_64bit && s.timestamp_valid && s.boot_session_id && + s.mcu_timestamp_us>UINT32_MAX)++a; + }); + auto second=g->imu().on_data([&](const auto& s) {if(s.timestamp_is_64bit)++b;}); + auto motor=g->motor().on_status([&](const auto&){++motors;}); + g->start_streaming(100,50); + EXPECT_EQ(fw.stream_config().source_mask,tp::StreamSrc::MotorStatus|tp::StreamSrc::ImuV2); + EXPECT_NO_THROW(g->start_streaming(100,50)); // logical source mask stays stable + for(unsigned i=0;i<100 && (!a||!b||!motors);i++)std::this_thread::sleep_for(std::chrono::milliseconds(5)); + EXPECT_GT(a.load(),0u);EXPECT_GT(b.load(),0u);EXPECT_GT(motors.load(),0u); + g->imu().off(first);g->imu().off(second);g->motor().off(motor); + g->close(); +} + +TEST(ImuTimePty, DuplicateLegacyFramesDoNotRestoreValidityOrRepeatCallbacks) { + Pty pty; + tx::bus::Transport transport(base_config(pty.slave_path())); + tx::IMU imu(transport); + std::mutex mutex; + std::condition_variable ready; + std::vector received; + auto id=imu.on_data([&](const auto& s) { + std::lock_guard lock(mutex); + received.push_back(s); + ready.notify_all(); + }); + pty.send_data(0,tp::Cmd::GetImu,packet(17173017,5113)); + for (int i=0;i<10;++i) + pty.send_data(0,tp::Cmd::GetImu,packet(3148,5114)); + pty.send_data(0,tp::Cmd::GetImu,packet(13148,5115)); + { + std::unique_lock lock(mutex); + EXPECT_TRUE(ready.wait_for(lock,std::chrono::seconds(2),[&] { + return !received.empty() && received.back().seq==5115; + })); + } + imu.off(id); + std::lock_guard lock(mutex); + ASSERT_EQ(received.size(),3u); + EXPECT_TRUE(received[0].timestamp_valid); + EXPECT_FALSE(received[1].timestamp_valid); + EXPECT_FALSE(received[2].timestamp_valid); +} diff --git a/cpp/tests/test_leader_exchange_pty.cpp b/cpp/tests/test_leader_exchange_pty.cpp new file mode 100644 index 0000000..247d11d --- /dev/null +++ b/cpp/tests/test_leader_exchange_pty.cpp @@ -0,0 +1,327 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// LeaderGripper::start_exchange() against the fake leader: polled mode with +// IMU/encoder/key fan-out to the ordinary subscribers, fallback on firmware +// without 0x5E, backlog draining, a lost reply replayed by same-seq retry +// rather than lost, strict one-request-at-a-time with concurrent queries, +// malformed replies, and stop. + +#include "fake_leader.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace tx = xense::taccap; +namespace tp = xense::taccap::protocol; + +using taccap_test::FakeLeader; +using taccap_test::Pty; + +namespace { + +constexpr auto kSecond = std::chrono::milliseconds(1000); + +std::unique_ptr open_leader(const Pty& pty) { + tx::LeaderGripper::Config cfg; + cfg.mcu_device = pty.slave_path(); + cfg.ack_timeout_ms = 300; + cfg.max_retries = 2; + return std::make_unique(cfg); +} + +template +bool wait_for(Pred pred, std::chrono::milliseconds budget) { + const auto deadline = std::chrono::steady_clock::now() + budget; + while (std::chrono::steady_clock::now() < deadline) { + if (pred()) return true; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + return pred(); +} + +// Collects delivered sequence numbers so a test can assert "nothing lost". +struct SeqLog { + std::mutex mu; + std::vector seqs; + void add(uint16_t s) { std::lock_guard lk(mu); seqs.push_back(s); } + std::size_t size() { std::lock_guard lk(mu); return seqs.size(); } + // Count of missing sequence numbers between first and last delivered. + unsigned gaps() { + std::lock_guard lk(mu); + unsigned missing = 0; + for (std::size_t i = 1; i < seqs.size(); ++i) { + const uint16_t d = static_cast(seqs[i] - seqs[i - 1]); + if (d != 1) missing += d == 0 ? 1u : d - 1u; + } + return missing; + } +}; + +} // namespace + +TEST(LeaderExchangePty, PolledModeFeedsImuEncoderAndKeySubscribersWithoutPush) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + + SeqLog imu, enc; + std::atomic keys{0}; + std::atomic last_key{-1}; + g->imu().on_data([&](const tx::ImuSample& s) { imu.add(s.seq); }); + g->encoder().on_data([&](const tx::EncoderSample& s) { enc.add(s.seq); }); + g->key().on_event([&](const tx::KeySample& k) { ++keys; last_key.store(k.key_state); }); + + ASSERT_TRUE(g->start_exchange(100, 100)); + EXPECT_TRUE(g->is_exchanging()); + EXPECT_TRUE(g->is_streaming()); + EXPECT_TRUE(fw.polled()); + EXPECT_EQ(fw.stream_mode(), static_cast(tp::StreamMode::Polled)); + ASSERT_TRUE(wait_for([&] { return imu.size() >= 30 && enc.size() >= 30; }, 2 * kSecond)) + << "imu=" << imu.size() << " enc=" << enc.size(); + + fw.press_key(tp::KeyState::DoubleClick); + ASSERT_TRUE(wait_for([&] { return keys.load() == 1; }, kSecond)); + EXPECT_EQ(last_key.load(), tp::KeyState::DoubleClick); + + EXPECT_EQ(fw.data_frames_pushed(), 0u) << "the leader pushed a DATA frame during the exchange"; + EXPECT_EQ(imu.gaps(), 0u); + EXPECT_EQ(enc.gaps(), 0u); + const auto s = g->exchange_stats(); + EXPECT_GE(s.exchanges, 20u); + EXPECT_EQ(s.timeouts, 0u); + EXPECT_EQ(s.malformed, 0u); + EXPECT_EQ(s.not_polled, 0u); + EXPECT_EQ(s.key_events, 1u); + EXPECT_EQ(s.records, s.imu_samples + s.encoder_samples + s.key_events + s.sensor_errors + s.other_records); + EXPECT_EQ(s.records_dropped, 0u); + + g->stop_streaming(); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_FALSE(g->is_streaming()); + EXPECT_FALSE(fw.polled()); + EXPECT_FALSE(fw.streaming()); + const unsigned after = fw.exchanges(); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + EXPECT_EQ(fw.exchanges(), after) << "exchange loop kept running after stop_streaming"; +} + +TEST(LeaderExchangePty, OldFirmwareFallsBackToTheOriginalStream) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + fw.set_exchange_supported(false); + auto g = open_leader(pty); + + std::atomic imu{0}; + g->imu().on_data([&](const tx::ImuSample&) { ++imu; }); + EXPECT_FALSE(g->start_exchange(100, 100)); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_TRUE(g->is_streaming()); + EXPECT_EQ(fw.stream_mode(), static_cast(tp::StreamMode::Separate)); + EXPECT_TRUE(wait_for([&] { return imu.load() >= 10; }, kSecond)); + EXPECT_GT(fw.data_frames_pushed(), 0u); + EXPECT_EQ(fw.exchanges(), 0u); + g->stop_streaming(); + EXPECT_FALSE(fw.streaming()); +} + +TEST(LeaderExchangePty, OldFirmwareWithoutFallbackThrowsAndLeavesNothingRunning) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + fw.set_exchange_supported(false); + auto g = open_leader(pty); + + EXPECT_THROW(g->start_exchange(100, 100, 0, false), tx::ProtocolError); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_FALSE(g->is_streaming()); + EXPECT_FALSE(fw.streaming()); + // The link is still usable for ordinary requests. + EXPECT_NO_THROW(g->transport().send_cmd(tp::Cmd::Heartbeat, {}, std::chrono::milliseconds(200))); +} + +TEST(LeaderExchangePty, RejectsBadArgumentsBeforeTouchingTheDevice) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + EXPECT_THROW(g->start_exchange(0, 0), tx::IoError); + EXPECT_THROW(g->start_exchange(100, 100, 501), tx::IoError); + EXPECT_FALSE(fw.streaming()); + g->start_streaming(100, 100); + EXPECT_THROW(g->start_exchange(100, 100), tx::IoError) << "must stop streaming first"; + g->stop_streaming(); +} + +TEST(LeaderExchangePty, HighRateRequestDefaultsToCappedExchangeRate) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + // A 1000 Hz request used to default exchange_hz to 1000 and throw. The cap + // itself is pinned in test_stream_rate.cpp (no wall-clock rate here: flaky + // on a loaded runner). + ASSERT_TRUE(g->start_exchange(1000, 100)); + EXPECT_TRUE(wait_for([&] { return fw.exchanges() >= 10; }, kSecond)); + g->stop_streaming(); + EXPECT_THROW(g->start_exchange(1000, 100, 501), tx::IoError) << "explicit > 500 still rejected"; +} + +TEST(LeaderExchangePty, StreamingCanKeepTheLegacyImuFrame) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + std::atomic imu{0}; + g->imu().on_data([&](const tx::ImuSample&) { ++imu; }); + + const unsigned before = fw.time_session_requests(); + g->start_streaming(100, 100, false); + EXPECT_EQ(fw.time_session_requests(), before) << "imu_v2_time=false must not negotiate a time session"; + EXPECT_TRUE(fw.stream_source_mask() & tp::StreamSrc::Imu); + EXPECT_FALSE(fw.stream_source_mask() & tp::StreamSrc::ImuV2); + EXPECT_TRUE(wait_for([&] { return imu.load() >= 10; }, kSecond)); + g->stop_streaming(); + + g->start_streaming(100, 100); // default keeps today's behaviour: negotiate + EXPECT_GT(fw.time_session_requests(), before); + g->stop_streaming(); +} + +TEST(LeaderExchangePty, BacklogIsFetchedAtOnceWithoutLoss) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + + SeqLog imu; + g->imu().on_data([&](const tx::ImuSample& s) { imu.add(s.seq); }); + // 100 Hz samples, 5 Hz exchanges: ~40 records (>504 B) per period. + ASSERT_TRUE(g->start_exchange(100, 100, 5)); + ASSERT_TRUE(wait_for([&] { return imu.size() >= 120; }, 3 * kSecond)) << imu.size(); + const auto s = g->exchange_stats(); + EXPECT_GT(s.continuation_exchanges, 0u); + EXPECT_EQ(s.records_dropped, 0u); + EXPECT_EQ(imu.gaps(), 0u); + g->stop_streaming(); +} + +TEST(LeaderExchangePty, LostReplyIsReplayedBySameSeqRetryNotLost) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + + SeqLog imu; + g->imu().on_data([&](const tx::ImuSample& s) { imu.add(s.seq); }); + ASSERT_TRUE(g->start_exchange(100, 100, 50)); + ASSERT_TRUE(wait_for([&] { return imu.size() >= 10; }, kSecond)); + + const auto timeouts_before = g->transport().stats().ack_timeouts; + fw.drop_exchange_replies(1); + ASSERT_TRUE(wait_for([&] { return fw.replays() >= 1; }, kSecond)); + const std::size_t at_replay = imu.size(); + ASSERT_TRUE(wait_for([&] { return imu.size() >= at_replay + 20; }, kSecond)); + EXPECT_GT(g->transport().stats().ack_timeouts, timeouts_before); + EXPECT_EQ(g->exchange_stats().timeouts, 0u) << "the retry recovered the reply"; + EXPECT_EQ(imu.gaps(), 0u) << "records from the lost reply were lost"; + g->stop_streaming(); +} + +TEST(LeaderExchangePty, NeverWritesWhileAReplyIsPendingEvenWithConcurrentQueries) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + fw.set_exchange_reply_delay_ms(3); + ASSERT_TRUE(g->start_exchange(100, 100)); + + std::atomic stop{false}; + std::atomic heartbeats{0}, heartbeat_timeouts{0}; + // 1 kHz of user queries: far above a real health check. An unthrottled + // back-to-back loop is a different case: the transport's half-duplex turn + // lock is not fair, so it can starve the exchange (see the SDK CHANGELOG). + std::thread user([&] { + while (!stop.load()) { + try { g->transport().send_cmd(tp::Cmd::Heartbeat, {}, std::chrono::milliseconds(200)); ++heartbeats; } + catch (...) { ++heartbeat_timeouts; } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + stop.store(true); + user.join(); + + EXPECT_GT(fw.exchanges(), 15u); + EXPECT_GT(heartbeats.load(), 5u); + // Only after giving up on a reply may the host write over it. + const auto gave_up = g->transport().stats().ack_timeouts; + EXPECT_LE(fw.overlaps(), gave_up) << "host wrote a frame while an exchange reply was pending"; + g->stop_streaming(); +} + +TEST(LeaderExchangePty, MalformedReplyIsCountedAndDispatchesNothing) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + std::atomic imu{0}; + g->imu().on_data([&](const tx::ImuSample&) { ++imu; }); + ASSERT_TRUE(g->start_exchange(100, 100, 20)); + ASSERT_TRUE(wait_for([&] { return imu.load() >= 5; }, kSecond)); + fw.corrupt_next_reply(); + ASSERT_TRUE(wait_for([&] { return g->exchange_stats().malformed >= 1; }, kSecond)); + EXPECT_TRUE(wait_for([&] { return imu.load() >= 30; }, kSecond)) << "exchange did not continue"; + g->stop_streaming(); +} + +TEST(LeaderExchangePty, FirmwareLeaseFallbackIsReArmedWithoutLoss) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + + SeqLog imu, enc; + g->imu().on_data([&](const tx::ImuSample& s) { imu.add(s.seq); }); + g->encoder().on_data([&](const tx::EncoderSample& s) { enc.add(s.seq); }); + ASSERT_TRUE(g->start_exchange(100, 100, 20)); // slow exchange: records are held when it fires + ASSERT_TRUE(wait_for([&] { return imu.size() >= 20; }, kSecond)); + EXPECT_EQ(fw.polled_starts(), 1u); + + fw.expire_lease(); + ASSERT_TRUE(wait_for([&] { return fw.lease_expiries() == 1; }, kSecond)); + ASSERT_TRUE(wait_for([&] { return fw.polled() && fw.polled_starts() == 2; }, kSecond)) + << "SDK did not re-enter the exchange"; + const std::size_t at_rearm = imu.size(); + ASSERT_TRUE(wait_for([&] { return imu.size() >= at_rearm + 30; }, 2 * kSecond)); + + const auto s = g->exchange_stats(); + EXPECT_GE(s.not_polled, 1u); + EXPECT_EQ(s.rearms, 1u); + EXPECT_EQ(s.rearm_failures, 0u); + EXPECT_EQ(s.timeouts, 0u); + EXPECT_EQ(imu.gaps(), 0u) << "samples lost across the fallback"; + EXPECT_EQ(enc.gaps(), 0u); + EXPECT_TRUE(g->is_exchanging()); + g->stop_streaming(); + EXPECT_FALSE(fw.streaming()); +} + +TEST(LeaderExchangePty, CloseEndsTheExchange) { + Pty pty; + ASSERT_GE(pty.master(), 0); + FakeLeader fw(pty); + auto g = open_leader(pty); + ASSERT_TRUE(g->start_exchange(100, 100)); + ASSERT_TRUE(wait_for([&] { return fw.exchanges() >= 3; }, kSecond)); + g->close(); + EXPECT_FALSE(g->is_exchanging()); + EXPECT_FALSE(fw.polled()); +} diff --git a/cpp/tests/test_monotonic_cv.cpp b/cpp/tests/test_monotonic_cv.cpp new file mode 100644 index 0000000..1ffaef6 --- /dev/null +++ b/cpp/tests/test_monotonic_cv.cpp @@ -0,0 +1,74 @@ +// Copyright (c) 2026 XenseRobotics Co., Ltd. — Apache-2.0 +// +// detail::MonotonicCondition / MonotonicTimedLock: timed waits on +// CLOCK_MONOTONIC (see monotonic_cv.hpp for why std:: ones are not enough on +// old-sysroot toolchains). Wall-clock steps cannot be produced without root, +// so this pins the contract; steady_clock == CLOCK_MONOTONIC is checked below. + +#include + +#include + +#include +#include + +namespace td = xense::taccap::detail; +using clk = std::chrono::steady_clock; +using namespace std::chrono_literals; + +TEST(MonotonicCv, TimesOutAfterTheDeadlineWithPredicateFalse) { + std::mutex m; td::MonotonicCondition cv; + std::unique_lock lk(m); + const auto t0 = clk::now(); + EXPECT_FALSE(cv.wait_for(lk, 30ms, [] { return false; })); + const auto dt = clk::now() - t0; + EXPECT_GE(dt, 30ms); + EXPECT_LT(dt, 500ms); + EXPECT_TRUE(lk.owns_lock()); +} + +TEST(MonotonicCv, NotifyWakesBeforeTheDeadline) { + std::mutex m; td::MonotonicCondition cv; bool ready = false; + std::thread th([&] { + std::this_thread::sleep_for(20ms); + { std::lock_guard g(m); ready = true; } + cv.notify_all(); + }); + std::unique_lock lk(m); + const auto t0 = clk::now(); + EXPECT_TRUE(cv.wait_until(lk, t0 + 5s, [&] { return ready; })); + EXPECT_LT(clk::now() - t0, 2s); + lk.unlock(); + th.join(); +} + +TEST(MonotonicCv, PredicateAlreadyTrueReturnsImmediately) { + std::mutex m; td::MonotonicCondition cv; + std::unique_lock lk(m); + EXPECT_TRUE(cv.wait_until(lk, clk::now() - 1s, [] { return true; })); +} + +TEST(MonotonicCv, SteadyClockIsTheMonotonicClock) { + // The helper hands steady_clock time points to a CLOCK_MONOTONIC cond. + timespec ts{}; + clock_gettime(CLOCK_MONOTONIC, &ts); + const auto mono = std::chrono::seconds(ts.tv_sec) + std::chrono::nanoseconds(ts.tv_nsec); + const auto steady = clk::now().time_since_epoch(); + EXPECT_LT(std::chrono::abs(steady - mono), 50ms); +} + +TEST(MonotonicTimedLock, ExcludesTimesOutAndHandsOver) { + td::MonotonicTimedLock lock; + ASSERT_TRUE(lock.try_lock_for(0ms)); + const auto t0 = clk::now(); + EXPECT_FALSE(lock.try_lock_for(30ms)) << "second owner admitted"; + EXPECT_GE(clk::now() - t0, 30ms); + std::atomic got{false}; + std::thread th([&] { got = lock.try_lock_for(2s); if (got) lock.unlock(); }); + std::this_thread::sleep_for(20ms); + lock.unlock(); + th.join(); + EXPECT_TRUE(got.load()); + EXPECT_TRUE(lock.try_lock_for(0ms)); + lock.unlock(); +} diff --git a/cpp/tests/test_stream_rate.cpp b/cpp/tests/test_stream_rate.cpp index 2c7f615..bea5b3a 100644 --- a/cpp/tests/test_stream_rate.cpp +++ b/cpp/tests/test_stream_rate.cpp @@ -93,3 +93,12 @@ TEST(StreamRate, NeverEnablesEskin) { const uint16_t all = det::stream_source_mask(100, 100, 100); EXPECT_EQ(all & (tp::StreamSrc::Eskin1 | tp::StreamSrc::Eskin2), 0u); } + +TEST(StreamRate, LeaderDefaultExchangeRateIsTheDataRateCappedAt200) { + using xense::taccap::detail::leader_default_exchange_hz; + EXPECT_EQ(leader_default_exchange_hz(100, 100), 100u); + EXPECT_EQ(leader_default_exchange_hz(0, 50), 50u); + EXPECT_EQ(leader_default_exchange_hz(200, 100), 200u); + EXPECT_EQ(leader_default_exchange_hz(1000, 100), 200u); + EXPECT_EQ(leader_default_exchange_hz(0, 0), 1u); +} diff --git a/cpp/tests/test_transport_reliability.cpp b/cpp/tests/test_transport_reliability.cpp new file mode 100644 index 0000000..c63d620 --- /dev/null +++ b/cpp/tests/test_transport_reliability.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include "pty_helper.hpp" +#include +#include +namespace tb = xense::taccap::bus; +namespace tp = xense::taccap::protocol; +using namespace std::chrono_literals; +using taccap_test::Pty; +using taccap_test::base_config; + +TEST(TransportReliability, WrongCommandCannotConsumePendingAck) { + Pty p; auto cfg = base_config(p.slave_path()); cfg.max_retries = 0; + tb::Transport host(cfg); + auto request = std::async(std::launch::async, [&] { + return host.send_cmd(tp::Cmd::GetVersion, {}, 500ms); + }); + auto f = p.expect_frame(); ASSERT_TRUE(f); + p.send_ack_ok(f->seq, tp::Cmd::Heartbeat); + EXPECT_EQ(request.wait_for(20ms), std::future_status::timeout); + p.send_ack_ok(f->seq, tp::Cmd::GetVersion); + EXPECT_EQ(request.get().cmd, tp::Cmd::GetVersion); + EXPECT_EQ(host.stats().unexpected_frames, 1u); +} + +TEST(TransportReliability, WrapCannotOverwritePendingRequest) { + Pty p; auto cfg = base_config(p.slave_path()); cfg.max_retries = 0; + tb::Transport host(cfg); + auto first = std::async(std::launch::async, [&] { + return host.send_cmd(tp::Cmd::GetVersion, {}, 2000ms); + }); + auto f = p.expect_frame(); ASSERT_TRUE(f); + for (unsigned i = 0; i < 256; ++i) { + host.send_cmd_no_ack(tp::Cmd::MotorImpedanceCtrl, {0}); + auto next = p.expect_frame(); ASSERT_TRUE(next); + EXPECT_NE(next->seq, f->seq); // active ACK slots are reserved even for no-ACK TX + } + auto second = std::async(std::launch::async, [&] { + return host.send_cmd(tp::Cmd::Heartbeat, {}, 500ms); + }); + auto g = p.expect_frame(); ASSERT_TRUE(g); EXPECT_NE(g->seq, f->seq); + p.send_ack_ok(g->seq, tp::Cmd::Heartbeat); + EXPECT_EQ(second.get().cmd, tp::Cmd::Heartbeat); + p.send_ack_ok(f->seq, tp::Cmd::GetVersion); + EXPECT_EQ(first.get().cmd, tp::Cmd::GetVersion); +} + +TEST(TransportReliability, DeliverySnapshotExposesUnacceptedTargetsWithoutExtraControl) { + Pty p; tb::Transport host(base_config(p.slave_path())); + xense::taccap::Motor motor(host); + EXPECT_EQ(motor.target_writes().count, 0u); + tp::MotorImpedanceCtrl target{}; + for (int i = 0; i < 3; ++i) { + motor.submit(target); auto f = p.expect_frame(); ASSERT_TRUE(f); + EXPECT_EQ(f->type, tp::FrameType::CMD_NO_ACK); + } + auto request = std::async(std::launch::async, [&] { return motor.delivery_snapshot(500ms); }); + auto f = p.expect_frame(); ASSERT_TRUE(f); EXPECT_EQ(f->cmd, tp::Cmd::GetMotorControlStats); + tp::MotorControlStats stats{}; stats.target_seq = 2; stats.applied_seq = 2; + std::vector data(sizeof(stats)); std::memcpy(data.data(), &stats, sizeof(stats)); + p.send_response(f->seq, f->cmd, data); + auto out = request.get(); + EXPECT_EQ(out.before.count, 3u); EXPECT_EQ(out.after.count, 3u); + EXPECT_EQ(out.firmware.target_seq, 2u); + EXPECT_GT(out.after.last_write_ns, 0); + EXPECT_GE(out.after.max_gap_us, out.after.last_gap_us); + EXPECT_FALSE(p.expect_frame(20)); // no automatic polling, target replay or stop +} + +TEST(TransportReliability, InheritedParityFlagsAreCleared) { + Pty p; int fd = ::open(p.slave_path().c_str(), O_RDWR | O_NOCTTY); + ASSERT_GE(fd, 0); termios tio{}; ASSERT_EQ(tcgetattr(fd, &tio), 0); + tio.c_iflag |= INPCK | IGNPAR; ASSERT_EQ(tcsetattr(fd, TCSANOW, &tio), 0); + { tb::Transport host(base_config(p.slave_path())); + ASSERT_EQ(tcgetattr(fd, &tio), 0); EXPECT_EQ(tio.c_iflag & (INPCK | IGNPAR), 0u); } + ::close(fd); +} + +TEST(DiagnosticsCodec, ExtendedErrorsRequireExplicitCompleteReply) { + tp::UartRxErrors errors{1,2,3,4,5,8,1234}; + std::vector wire(72, 0); std::memcpy(wire.data()+44, &errors, 28); + auto out = tp::decode_uart_rx_errors(wire.data(), wire.size()); + EXPECT_EQ(out.overrun, 1u); EXPECT_EQ(out.framing, 2u); EXPECT_EQ(out.noise, 3u); + EXPECT_EQ(out.parity, 4u); EXPECT_EQ(out.receiver_timeout, 5u); + EXPECT_EQ(out.last_error, 8u); EXPECT_EQ(out.last_error_ms, 1234u); + EXPECT_THROW(tp::decode_uart_rx_errors(wire.data(), 44), xense::taccap::ProtocolError); + EXPECT_THROW(tp::decode_uart_rx_errors(wire.data(), 71), xense::taccap::ProtocolError); + EXPECT_THROW(tp::decode_uart_rx_errors(nullptr, 72), xense::taccap::ProtocolError); +} + +TEST(TransportReliability, ExtendedErrorsAreOptInAndOldFirmwareIsNotZeroFilled) { + Pty p; tb::Transport host(base_config(p.slave_path())); + xense::taccap::Diagnostics diag(host); + auto first = std::async(std::launch::async, [&] { return diag.uart_stats(500ms); }); + auto f = p.expect_frame(); ASSERT_TRUE(f); EXPECT_TRUE(f->payload.empty()); + p.send_response(f->seq, f->cmd, std::vector(44)); + EXPECT_EQ(first.get().rx_errors, 0u); + auto second = std::async(std::launch::async, [&] { return diag.uart_rx_errors(500ms); }); + f = p.expect_frame(); ASSERT_TRUE(f); EXPECT_EQ(f->payload, std::vector{1}); + p.send_response(f->seq, f->cmd, std::vector(44)); + EXPECT_THROW(second.get(), xense::taccap::ProtocolError); +} diff --git a/docs/FIRMWARE.md b/docs/FIRMWARE.md index f7167b5..c7179c6 100644 --- a/docs/FIRMWARE.md +++ b/docs/FIRMWARE.md @@ -7,6 +7,56 @@ > `GetVersion` 查（`gripper.firmware_version`）——它返回的是编译进镜像的常量， > 是唯一能证明刷进去了什么的东西。 +## 本地候选：IMU 单调时间与协议兼容 + +固件与 SDK 当前本地分支均为 `codex/gripper-imu-support`，由原从爪分支延续。 +主爪也启用 TIM2 单调时间和 V2，并只发送新缓存样本；请求 1000 Hz 不代表 +1000 Hz 独立采样。主爪原校准和从爪电机控制算法不在本轮修改范围。 + +尚未正式发布。新固件支持 `TimeSession (0x16)` 和 `GetImuV2 (0x17)`； +SDK 的 `read_once()` 和主/从爪显式 IMU streaming 自动协商： +明确 InvalidCmd 才使用旧 28 字节包；超时/忙/畸形响应报错，不静默降级。 +从爪默认仍只开电机状态流，主爪现有 `start_streaming` 参数不变。 +V2 使用 source_mask=0x0020，保持电机与编码器 source 位和速率不变。 + +新采样字段（C++ 与 Python）： + +| 字段 | 含义 | +| --- | --- | +| `mcu_timestamp_us` | C++ uint64_t / Python int；V2 完整捕获时间，旧包为首次观测起的展开时间 | +| `boot_session_id` | V2 本次启动的非零会话号；旧包为 0，应与设备 SN 联合使用 | +| `timestamp_is_64bit` | 是否来自原生 V2，而不是 SDK 展开 | +| `timestamp_valid` | 本帧时间是否可用于当前观测段；异常旧包跳变或 V2 乱序为 false | +| `timestamp_discontinuity` | 会话改变或不确定跳变，消费者应重置积分/滤波/时间拟合 | + +会话号由 SDK 提议随机 64 位 nonce，固件每次启动只接受第一个，保存在 RAM； +重复协商不改变它，不写 Flash。设备重启后必须重新协商、重开流。 +会话号是概率唯一的诊断标识，不是认证凭据。V2 可跨任意多次 32 位回绕和断流恢复， +但重启会重置时间，应按会话分段，不能把新启动拼成旧启动的连续轨迹。 + +旧包只在短间隔、序列正常的连续观测下展开自然回绕。旧 DWT 固件的约 17 秒回退、 +重启样跳变和超过约 35.8 分钟的观测空档标记 discontinuity/invalid，不制造假时间。 +检测到上述旧包异常后，该时间线保持 invalid/discontinuity，不因下一帧重复或正常前进 +自动恢复。流可 `off(id)` 后重新 `on_data(...)` 建立新观测段；轮询需新建 IMU 实例。 +有效原生 V2 可恢复可信时间。重新建立观测段不保证恢复绝对开机时间，不能将两段直接积分； +旧 DWT 固件仍会再次回退，根治需要升级固件。旧包无法排除所有重启歧义。 +`IMU::decode` 是无状态解码；`read_once` 与每个 `on_data` 订阅分别维护时间线。 + +`on_data` 现在过滤连续字节完全相同的 IMU DATA 载荷，减少缓存重复引起的重复积分； +原始传输订阅与 `read_once` 不过滤。多个订阅者各自保有去重及时间线状态。 +回调频率因此可能由约 1000 Hz 降至约 100 Hz；不要把回调次数当成串口接收帧数。 +使用者必须检查 `timestamp_valid`，无效样本不能用于积分；这不是坐标系或精度修正。 + +**集成限制：**这是 C++ `ImuSample` 布局变更，所有链接消费者必须重编译，不能仅替换共享库。 +Python 扩展也须重建并核实实际加载路径。ROS 的时间字段须为 uint64，并增加会话/有效性字段， +否则旧 uint32 消息仍会截断；本任务不修改独立 ROS 仓库或其依赖副本。 +ROS header 时间仍是另外的时钟域，以上修改不代表相机/IMU 同步通过验收。 +固件/SDK 本地构建成功也不等于已刷设备或安装到业务环境。 + +软件检查：`ctest --test-dir --output-on-failure`、`python -m pytest python/tests -q`。 +真机还需超过 75 分钟连续采集与冷重启验收；默认 motor-only、旧 SDK、新旧协议互通、 +IMU 同时上报时的控制时延和 OTA 都需验证。IMU 数值精度另行验收。 + ## Firmware / PC GUI reference repos The wire protocol this SDK speaks is defined by the firmware that runs @@ -29,18 +79,18 @@ What's in them: - `tc-gu-01/App/protocol/protocol_cmd.h` + `protocol_data.h` — canonical command enum + POD payload layouts. The SDK's `cpp/include/taccap/protocol/{commands.hpp,payloads.hpp}` mirror these - 1:1 with `static_assert(sizeof(...) == ...)` size checks. Currently - mirrored through command `0x5B` (protocol doc V2.6: V2.2 plus the - 0x54 / 0x55 diagnostic pair, 0x56–0x58 motor spec / homing diagnostics / - motor version, 0x59 / 0x5A motor model, 0x5B CAN extended-frame relay); - `scripts/check_protocol_drift.py` is green against firmware `07ab9bb`. + with compile-time and cross-repository size checks. SDK PR #29 compares + against firmware PR #23 (`refs/pull/23/head`), which defines the time, + IMU V2 and exchange commands. `TxTrace` (0x18) is reserved for diagnostic + firmware builds; its enum value does not imply support in ordinary images. + Other CI runs use `hw_v1.1.0`. If the firmware PR changes independently, + rerun the SDK mirror job before merging; the log records the tested revision. - `tc-gu-01/App/protocol/PROTOCOL_SPEC.md` + `tc-gu-01/docs/PROTOCOL.md` — the human-readable spec, including the §10 offset table for the 72-byte extended motor status that `test_codec_v22.cpp` is transcribed from. - `tc-gu-01/App/tasks/task_data_stream.c` + `task_imu.c` + - `task_encoder.c` — explains why IMU/encoder unique-data rate caps at - ~60 Hz even when you request 100 (see the SDK's stream-dup note in - the Claude memory). + `task_encoder.c` — acquisition, cached samples and stream scheduling are + separate; requested rate is not evidence of independent sample rate. - `tc-gu-01-pc/core/protocol.py` + `core/serial_worker.py` — Python reference implementation of the same wire protocol; useful as a cross-check when debugging the C++ codec. @@ -76,11 +126,11 @@ inactive bank and uses the STM32H5 bank swap, so one build serves both banks. ```bash python python/examples/ota_update.py \ third_party/firmware/tc-gu-01/build/master/tc-gu-01-master.bin \ - left --target-version 1.2.6 + left --target-version 1.2.7 ``` **The two roles have independent version numbers** — at the time of writing the -leader is 1.2.6 and the follower 1.2.16, and neither is "behind" the other. Both +leader is 1.2.7 and the follower 1.2.17, and neither is "behind" the other. Both carry the same control-UART receive fix, which lives in shared code. The roles were briefly forced onto one number; that was dropped on 2026-09-24. @@ -92,13 +142,13 @@ them apart. If a leader reports 1.2.6, check `g.diagnostics.uart_stats()`: the fixed image answers with 44 bytes, so `rx_errors` / `rx_rearms` exist and are meaningful; the old one answers 32 bytes and the SDK reads both as 0 -- which is indistinguishable from a healthy zero. When in doubt, reflash -`tc-gu-01-master-1.2.6.bin` from `firmware/`; OTA compares no versions +the current leader image from `firmware/`; OTA compares no versions (`--target-version` is informational), so reflashing the same number is fine. Compare versions only within one role. Note the build output keeps the Makefile's unversioned name (`build/master/tc-gu-01-master.bin`), while the images released under -`firmware/` carry the version (`tc-gu-01-master-1.2.6.bin`). That is deliberate: +`firmware/` carry the version (`tc-gu-01-master-1.2.7.bin`). That is deliberate: a build artifact is whatever you just compiled, a release is a specific version someone may still be holding a copy of months later. **If you promote a local build into `firmware/`, rename it and update `firmware/manifest.json` in the @@ -113,20 +163,32 @@ are told to use. `test_ota_update_all.py` fails if they disagree. > 复位。在一台 EL05 从爪上实测:拔 24V 的瞬间(<0.3 秒)板载 USB 集线器连同串口芯片 > 一起掉线、MCU 重启,从最快的一碰到断开 10 秒,15/15 次都如此;OTA 后只断一次 24V, > 60 秒状态流 6000 帧一帧不丢。**主爪**没有 24V:拔插它的 USB 即可。 -> 确认真的断过电:看 `gripper.device.heartbeat().uptime_ms` 是否从接近 0 重新计起。 -> `/dev/serial/by-id/` 的时间戳通常也会跟着变,但 uptime 才是 MCU 真的重启过的直接证据。 +> 记录现场断开并重接 24V 的操作，再检查 `gripper.device.heartbeat().uptime_ms` 是否重新计时。 +> uptime 重置只证明 MCU 重启，软复位也会重置它；不能单凭 uptime 或 +> `/dev/serial/by-id/` 的时间戳证明串口桥已经断电。 > -> bank-swap 重启是软复位：MCU 重新初始化，但片外的 USB 转串口桥从没断过电。设备 -> 回来之后停在一个降级状态，而这个状态和健康状态**从任何可观测的角度都分不出来** -> —— 版本号正确、数据流在跑、`uart_stats()` 的收发计数全是干净的。唯一的症状是它 -> 在悄悄丢状态帧。 +> bank-swap 重启是软复位：MCU 重新初始化，片外 USB 转串口桥可能仍保持供电。 +> 历史实测曾出现升级后丢状态帧，而版本号、流启动状态和 `uart_stats()` 计数正常的情况。 +> 这些检查不足以证明数据完整；仍需检查帧间隔、序号和解析器统计。 > > 实测，同一只夹爪、同一个固件、同一条线、60 秒一轮：仅 OTA 之后每轮丢 35~39 帧， > 断电重插之后连续三轮为 0。 > > 在这上面栽过两次：先把它误判成"某只夹爪链路劣化、疑似硬件损坏"，又用两组都没 -> 断电的数据去比较固件版本，得出了并不存在的版本差异。**任何在断电之前取的数字 -> 都不可信。** +> 断电的数据去比较固件版本，得出了并不存在的版本差异。断电状态应记录为测试条件。 + +> 冷启动后仍需在相同负载下复验；断电不能代替故障定位。关闭 IMU、只接收上报、 +> 同时发送查询应分开对照。若 `read()` 原始字节中已出现短帧，只能确定损坏发生在 +> SDK 帧解析之前，仍需 USB/UART 或固件侧证据区分原因。不要把 `crc_errors=0` +> 当成无丢帧：长度检查可以先于 CRC 拒绝短帧。 +> +> 2026-10-26 的另一轮实测在确认断电后，仍在双向通信负载下复现接收短帧：USBmon 接收载荷与 +> 串口 `read()` 的 843445 字节逐字节一致，131 条协议坏记录已经出现在两者中。 +> 这排除了本次 SDK 解析或 TTY 接收额外删字节的解释，但 USBmon 位于 HCD/驱动 +> 边界，并不是 UART 引脚或 USB 线上的物理抓取，不能据此直接认定桥接芯片或固件责任。 +> 查询 `gripper.diagnostics.uart_stats()` 时需要调用方法；只访问方法对象不能获得计数。 +> MCU `tx_bytes_ok` 与主机接收字节对账应在停流、排空后取快照，并使用包含前一个统计 ACK、排除后一个统计 ACK 的 +> 原始接收字节区间；`tx_bytes_ok` 是 HAL 成功返回时累计的请求长度，也不能替代引脚观测。 > Only builds you made yourself need that path. To flash the **released** > images, name them and let the script find them in [`firmware/`](../firmware/) — diff --git a/docs/IMU_MOTOR_TESTING.md b/docs/IMU_MOTOR_TESTING.md new file mode 100644 index 0000000..09df553 --- /dev/null +++ b/docs/IMU_MOTOR_TESTING.md @@ -0,0 +1,261 @@ +# 从爪 IMU 与电机控制完整性、稳定性测试 + +本文是可重复执行的测试方案，不是通过报告。软件测试、当前机器上的 SDK 加载状态、设备上实际运行的固件，以及机械运动验收必须分别记录结果。未执行的项目标为 `NOT RUN`，设备或条件不具备时标为 `BLOCKED`。 + +方案针对支持 `FollowerGripper.start_streaming(motor_hz=100, imu_hz=...)` 的 SDK，以及配套固件的从爪 IMU 功能。编写时核对的基线为 SDK `c06bc342ba0c534f0e830e5ef8574e4fc0ba5d1f`、固件仓库 `XenseRobotics-AI/tc-gu-01` 的 `codex/gripper-imu-support@1d43fcaa423aca515ca4ef666e6e605fc5d4a5`；执行时记录实际 SHA 和未提交修改，不把这两个历史核对值当作“最新”。 + +## 1. 能确认什么，仍须测量什么 + +当前设计默认只上报电机状态；显式设置 `imu_hz=1..100` 才请求 IMU。SDK 先读 IMU 探测能力，再协商时间格式和修改流；旧固件拒绝时不会先改写流。实现见 [follower_gripper.cpp](../cpp/src/follower_gripper.cpp)、[imu.cpp](../cpp/src/components/imu.cpp)。 + +配套固件 `App/tasks/task_imu.c`、`task_slave_motor_control.c`、`task_data_stream.c`、`slave_imu.c` 中，电机控制、数据流、从爪 IMU 的线程优先级分别为 8、11、12；电机控制周期为 2 ms，电机状态最高 100 Hz；同时到期时先发电机状态。IMU 采样失败不将旧包伪装成新样本，也不主动停止电机控制。这些设计降低了干扰，但共享 CPU、UART 和主机处理仍可能产生时延。 + +当前修复版将 SDK 内部电机控制反馈放入独立有界队列和工作线程；普通 motor/IMU 用户回调仍共享用户分发线程，慢回调会延迟用户观测，但不再阻塞两个控制器的反馈入口。控制队列溢出会锁存关闭该通道、停止继续交付反馈，由控制器现有状态超时逻辑进入 Fault/零输出；必须重建 Transport 才能清除锁存，不能自动恢复运动。`control_queue_overflows`、`control_queue_dropped` 和 `control_queue_high_water` 可观测此边界。 + +控制回调还会按现有策略的严重故障位、非有限数值及超量程力矩条件锁存安全异常，避免同批次的后续健康帧覆盖故障；判据及阈值不变，日志与策略处理仍在控制器线程。属于当前控制会话但已过期的故障可锁存，却不能刷新反馈年龄；显式 `reset()` 建立新接收边界，忽略 reset 前积压帧。 + +Frame/ACK 的本机单调接收时间贯穿 IMU、motor、encoder 样本至控制器；完整帧以其最后字节所在 `read()` 返回时刻为准，不再在延迟分发时重标为“现在”。控制器拒绝过期、未来或倒序的反馈。`host_time` 仍不是硬件采样/相机曝光时间，串口之前的缓存延迟和 MCU 重复缓存反馈仍须结合设备时间戳判断。直接调用静态 `decode()` 无接收上下文，保留调用时刻的旧行为。 + +历史 `c06bc34` 的共享控制/用户队列及分发时刻计龄缺陷，保留在已有测试报告中，不应当作修复版结果。需同源重建 C++ core 和 Python native（结构布局已扩展），不能把新 Python 与旧 `.so` 混用。软件替身测试不证明真实 RTOS 抢占、SPI/CAN/UART 时序、电源稳定或夹爪运动质量；仍须测量设备反馈时间、主机接收/分发积压和目标更新节奏。 + +### 原始接收证据（默认关闭） + +`g.transport.enable_receive_trace(True)` 开启 SDK 解析前字节与拒帧记录，存入最多 256 条内存环；不在接收线程调用用户回调或写磁盘。完成采集后建议先停流，再调用 `g.transport.take_receive_trace()` 导出、随后 `enable_receive_trace(False)` 关闭。导出结果含 `records` 和 `overwritten_records`：覆盖计数不为零说明早期记录已丢失，不能宣称完整捕获。 + +每条记录给出 `kind`（input/reject）、`host_time_ns`、全局字节 `offset`、`available`、`consumed` 及 `bytes`。原始输入较大时拆分保存；拒帧保存最多 1024 字节前缀，按 offset 对齐 input。`reason` 数值：0 无错误、1 无帧头、2 超长、3 短正文、4 长度不符、5 CRC不符。Transport Stats 同时提供 `parser_no_head/oversize/short_body/bad_length`；长度检查先于 CRC，故 `crc_errors=0` 不能排除损坏输入。此证据只能证明字节到达 SDK 之前的状态；MCU 发送、桥接器、线路责任仍需独立抓取对齐。 + +## 2. 设备、环境与运行身份 + +每次运行记录以下内容，EL05 与 RS00、左右爪分别建档，不用一种设备结果替代另一种： + +| 类别 | 必须记录 | +| --- | --- | +| SDK | 仓库分支、完整 SHA、dirty 状态、构建配置、Python 可执行文件、Python 包与 `_taccap_native` 实际加载路径、版本 | +| 固件 | MCU 返回版本、固件构建 SHA、烧录 BIN 的 SHA-256、烧录记录；电机型号及其固件版本来源 | +| 设备 | 精确 firmware SN、角色和左右侧、CH343 序列号、MCU 的 by-id 路径、硬件版本 | +| 台架 | 电源电压/限流、线缆与 USB 拓扑、夹持物/治具、环境温度、传感器安装方向、外部急停与独立测量仪器 | +| 控制 | 控制器类型、完整配置、行程标定、安全包络、实际轨迹、循环次数、驻留时间、负载和温度停止条件 | + +配套试验固件曾沿用从爪 `1.2.16`，因此相同版本号不代表相同程序，也不能证明 IMU 或时间扩展已刷入。能力探测只能证明被探测接口可用；没有烧录证据时将固件 SHA 记为 `unknown`，不能用本机源码 SHA 填充。 + +运行前核对独立构建/测试环境。构建源码不等于当前 Python 已加载它；尤其要检查原生扩展路径，避免旧 `.so` 与新 Python 代码混用。构建与安装操作按 [INSTALL.md](INSTALL.md) 执行；本文采集片段不自动安装或刷机。 + +### 串口与发现副作用 + +`scan_grippers()` 不是被动 USB 枚举：它只选择 `/dev/serial/by-id/usb-1a86_USB_Dual_Serial_*-if02`，以 3,000,000 baud 打开控制口，发送 `StopStream`、`GetDevType`、`GetSn` 后关闭。不要根据 `/dev/ttyACM0`、`ttyACM1` 编号猜测 MCU 接口，也不要试开另一接口。实现见 [discovery.cpp](../cpp/src/discovery.cpp)。 + +SDK 使用 `flock` 与 `TIOCEXCL` 独占串口，`in_use` 必须视为占用并退出，不能抢占、杀死现有控制进程或绕过锁。发现和构造 `FollowerGripper` 都可能停止遗留流，应在设备原控制任务已正常结束、机械状态安全后进行；独占锁不能代替现场确认。 + +`FollowerGripper` 默认构造会停流、读取版本/SN/电机型号/行程配置，部分设备还读取 CAN ID；默认不开相机，不使能电机、不发运动目标、不写 Flash。它不凭构造参数证明设备角色，仍须根据发现结果确认唯一 SN 的 `Role.Follower`。上电自动回零属于固件行为，不能因采集脚本没有运动命令就忽略正在进行的回零。 + +## 3. 分阶段测试矩阵 + +各阶段先记录预期、门槛和停止条件，再执行；表中时长是建议的采样预算，可按产品要求调整，不是已完成次数。 + +| 阶段 | 场景与步骤 | 主要证据与进入下一阶段条件 | +| --- | --- | --- | +| S0 软件回归 | 运行第 8 节 C++/Python 测试；有固件源码时运行其主机替身测试 | 通过接口、混合数据流、时间、并发和退出检查；缺依赖/失败保留原始结果 | +| H0 硬件预检 | 核实唯一设备、启动终态、方向与行程标定、型号、包络、实际已失能 | `home_diag()` 无 ACTIVE，状态/故障可读且新鲜；角色、固件或机械状态未知时不运动 | +| H1 静止上报 | motor=100 Hz；IMU=0、10、50、100 Hz，5 s 预热后每段采集 60 s，A/B/A 重复至少 3 次 | 有效电机/IMU 速率、时延分位数、时间连续性、错误计数；确认开 IMU 与恢复 motor-only 后均稳定 | +| H2 空载小行程 | 采用已确认的行程内部范围，先少量慢速往返，再每条件不少于 10 次；两类控制器分别做同轨迹 A/B/A | 到位误差、轨迹完成时间、速度纹波、反向帧、峰值力矩、温升、控制 deadline 和目标年龄 | +| H3 代表性负载 | 明确夹持物、接触位置、摩擦/柔顺条件；重复 H2，再测试规定保持时长 | 力矩/温升、滑移或掉落、包络行为；需要夹持力结论时使用标定的力传感器 | +| H4 长时间稳定性 | 初筛通过后 100 Hz IMU 连续 30–60 min；产品验收再按任务周期做 8 h 或规定时长；前后追加 IMU=0 对照 | 每段数据量、错误计数、温度、RSS/CPU、重连/复位/意外失能、轨迹完成率；长稳通过仅覆盖实测时长 | +| H5 生命周期 | 停止后切换 IMU 频率、重复开/关和连接/退出；安全停机后冷启动，分别测工具提前连接和启动后连接 | 启动时间、回零结果、IMU 就绪时间、端口释放、重复会话无回调残留；不能将 MCU 软复位代替整机断电 | +| H6 故障/保护 | 独立急停、人员在场、无坠落危险的专用台架上测试 host 中断/断链、IMU 故障、限位/堵转 | 故障可见、停机时间与最终状态、恢复需明确动作且无意外运动；不在普通长稳循环自动注入 | +| H7 校准维护 | 运动中请求校准应拒绝且不停止控制；停止失能后在指定姿态单独执行校准、保存、重启恢复 | BUSY 响应、控制连续性、偏置复测和参数恢复；持久化/断电故障另列步骤，不自动混入采集 | + +H6/H7 的断电、传感器断开、堵转、校准保存、OTA 等操作具有实际设备副作用。执行前需明确设备、动作、机械条件和恢复方式；普通数据采集流程不执行这些动作。**慢回调/分发积压只在 PTY 或已安全失能的场景注入；真机运动不做该注入，除非独立安全机制和运动范围已经明确。** 记录预期触发的故障与非预期故障，不能用正常阶段的“错误必须为零”否定有意故障注入，也不能把注入阶段的错误掩盖到正常基线里。 + +### A/B/A 控制变量 + +A 为同一候选固件、同一 SDK 的 `imu_hz=0`；B 分别为 10、50、100 Hz；再次 A 检查温升和时间漂移。建议 `0→10→0→50→0→100→0`，重复时交替 B 的顺序。若还要评估固件改动本身，需要另测已知基线固件，刷机与冷启动应单独授权和记录。 + +保持 motor_hz=100、控制器参数、轨迹、负载、电源、主机负载、USB 拓扑、日志配置和诊断查询时点一致。改变 IMU 频率时先安全停止控制器、停流，再重新配置。不要在运动中反复 `start_streaming()`，也不要把切流停顿算作稳态抖动。 + +## 4. API、控制顺序与退出 + +推荐接口如下，定义见 [Python bindings](../python/bindings/bind_gripper.cpp)、[motor bindings](../python/bindings/bind_motor.cpp)、[control bindings](../python/bindings/bind_control.cpp)： + +| 用途 | API | +| --- | --- | +| 发现与打开 | `scan_grippers()`；`FollowerGripper(mcu_device=endpoint.mcu_device, open_cameras=False)` | +| 预检 | `g.home_diag()`、`g.get_gripper_config()`、`g.motor.get_model()`、`g.motor.get_spec()`、`g.audit_envelope()` | +| 订阅 | `g.motor.on_status(callback)`、`g.imu.on_data(callback)`；保存订阅 ID 并使用对应 `off(id)` | +| 配置流 | `g.start_streaming(motor_hz=100, imu_hz=rate)`；改频率前 `g.stop_streaming()` | +| 主机控制 | `ForcePositionConfig.for_spec(g.motor.get_spec())` / `ImpedanceConfig.for_spec(...)`，对应 Controller 的 `start()`、`set_target()`、`snapshot()`、`stop()` | +| 显式使能 | 控制器 `start()` 成功后，由调用者执行 `g.motor.enable()`；`start()` 本身不使能 | +| 诊断 | `g.motor.control_stats()`、`g.diagnostics.uart_stats()`、`g.transport.stats` | +| 退出 | `controller.stop()` → `g.stop_streaming()` → 注销回调 → `g.close()` | + +**先开 motor+IMU 共流，再 `controller.start()`，成功后显式 `g.motor.enable()`。** 两种控制器会借用已开启的流；若先启动控制器，默认是 motor-only，之后不能直接新增 IMU。控制器启动会读取并校验电机限制、以当前位置初始化目标，但不使能电机；应在使能前完成这些启动检查。控制时不得预开 IMU-only：控制器需要电机状态反馈。借用的流需由调用者最终关闭。 + +回调只做有界、快速的数据复制/入队。不要在 IMU 或电机回调中 `sleep`、同步写磁盘、绘图、打印、获取长期持有的锁或发送 ACK 查询；另设消费线程批量保存，并记录采集队列溢出。`queue_dropped=0` 也不证明回调无害，必须同时测量 motor 间隔和控制状态。 + +运动中从流和 `controller.snapshot()` 获取状态；不要频繁 `read_status()`、`read_status_ext()`、`fault_report()` 或重新 `scan_grippers()`。诊断 ACK 同样占用链路，应放在统一区段边界；需要运行中诊断时采用一致、已记录的低频查询，并区分查询附近窗口。 + +`close()` 释放端口但不失能电机。控制器 `stop()` 会尝试零输出和失能，但失能失败可能只记录警告；断链时不能以函数返回证明物理停机。运动测试退出后在链路仍可用时确认已失能/实际反馈模式，再关闭；断链使用台架独立急停及恢复流程。无运动采集不应为了“清理”而擅自对正在夹持的设备发失能。 + +### 默认无运动的采集片段 + +以下片段在确认设备原任务已经结束后使用。它不会使能、设目标、校准、写 Flash 或刷机；发现/构造和开停流具有前述副作用。先把代码保存为本次运行脚本，再传入精确 SN；没有 SN 时会在发现设备前退出。为便于核对只输出段摘要，正式验收应按第 6 节保存原始数据与完整诊断差分。 + +```python +import collections +import sys +import time +from xense.taccap import FollowerGripper, Role, scan_grippers + +if len(sys.argv) != 2: + raise SystemExit("usage: python capture.py ") +endpoints = scan_grippers() +matches = [e for e in endpoints if e.firmware_sn == sys.argv[1]] +if len(matches) != 1 or matches[0].role != Role.Follower or matches[0].in_use: + raise SystemExit("Need exactly one available follower with the requested SN") + +g = FollowerGripper(mcu_device=matches[0].mcu_device, open_cameras=False) +motor_sub = imu_sub = None +motor_rows = collections.deque(maxlen=65536) +imu_rows = collections.deque(maxlen=65536) +try: + print("firmware", g.firmware_version, "SN", g.firmware_sn) + if g.home_diag().flags & 0x01: # HomeDiagFlag.Active + raise RuntimeError("Power-on calibration is active; end this run") + initial = g.motor.read_status() + if initial.status & 0x01: # MotorStatusBit.Enabled + raise RuntimeError("Motor already enabled; require a safe stopped session") + motor_sub = g.motor.on_status(lambda s: motor_rows.append(( + time.monotonic(), s.host_time, s.raw.status_timestamp_ms, + s.actual_pos, s.actual_vel, s.actual_torque, s.motor_temp_c, + s.status, s.fault_code, s.latched_fault_code))) + imu_sub = g.imu.on_data(lambda s: imu_rows.append(( + time.monotonic(), s.host_time, s.mcu_timestamp_us, s.seq, + s.boot_session_id, s.timestamp_is_64bit, s.timestamp_valid, + s.timestamp_discontinuity, s.valid_flag))) + for rate in (0, 10, 0, 50, 0, 100, 0): + before_uart = g.diagnostics.uart_stats() + before_transport = g.transport.stats + g.start_streaming(motor_hz=100, imu_hz=rate) + time.sleep(5.0) + begin = time.monotonic() + time.sleep(60.0) + end = time.monotonic() + g.stop_streaming() + # A timestamp window excludes preheat and any late-dispatched samples. + motor = [r for r in list(motor_rows) if begin <= r[0] <= end] + imu = [r for r in list(imu_rows) if begin <= r[0] <= end] + after_uart = g.diagnostics.uart_stats() + after_transport = g.transport.stats + print({"imu_hz": rate, "seconds": end - begin, + "motor_samples": len(motor), "imu_samples": len(imu), + "motor_hz_observed": len(motor) / (end - begin), + "imu_hz_observed": len(imu) / (end - begin)}) + # Serialize the rows and before/after counters outside the callbacks. + # This short example's 65536-row buffers cover its planned duration; + # a long run needs a draining queue plus an explicit overflow counter. +finally: + try: + g.stop_streaming() + finally: + try: + if motor_sub is not None: + g.motor.off(motor_sub) + if imu_sub is not None: + g.imu.off(imu_sub) + finally: + g.close() +``` + +### 运动配置必须先填写 + +行程用归一化 `[0, 1]` 表示，原始轴角为 rad、轴角速度为 rad/s、力矩为 N·m。归一化开度不直接等于 mm；须使用该装置的机构/标定映射。电机力矩不能未经传动比、有效力臂、效率/摩擦、接触点与实测标定直接换算成指尖夹持力 N。空载初筛采用行程内部范围，避免把端点预载或硬限位作为普通轨迹。 + +运行记录中先填写下表；空值表示不能开始该项运动。`for_spec()` 的默认值反映电机型号，不等于本次台架已批准的负载、速度或力矩。 + +| 参数 | 待填写的本次值 | +| --- | --- | +| 精确 SN、左右侧、EL05/RS00、控制器 | `待填写` | +| 起点/终点归一化开度，允许 raw rad 范围 | `待填写` | +| 起止姿态、夹爪固定方式、运动区域和负载 | `待填写` | +| 速度 rad/s、ForcePosition.grasp_torque_nm 或 Impedance.max_position_torque_nm | `待填写` | +| Impedance 的 kp/kd/feedforward 或 ForcePosition 的预载、保护配置 | `待填写完整配置` | +| 每段驻留、往返次数、完成时限、允许位置误差 | `待填写` | +| 最大温度、力矩、电流及独立急停条件 | `待填写` | + +运动时顺序为：确认已失能，读取/审核型号与包络，配置控制器，订阅轻量采集回调，`g.start_streaming(motor_hz=100, imu_hz=rate)`，再 `controller.start()`；启动成功且所有运动前提满足后，显式调用 `g.motor.enable()`，随后在规定范围内 `set_target()`，以 `snapshot()` 和流检验轨迹。两类控制器的 `start()` 都不调用电机使能，不能省略或提前执行这个独立步骤；对应示例为 [impedance_control.py](../python/examples/impedance_control.py) 和 [force_position_control.py](../python/examples/force_position_control.py)。每段停止前取得结束控制统计，再执行本节退出顺序；相邻段恢复同一初始条件。 + +## 5. 时间、帧率和序列号的解释 + +- IMU 当前固件开流后约每 10 ms 采样，上报设置为 10/50/100 Hz 时，采样线程仍约 100 Hz。`seq` 是 16 位采样序号，低上报频率自然会跨号；先按设备时间和抽样倍率解释，不能直接把所有跨号加成丢包数。遇到回绕用无符号模差，遇到重启先切分会话。 +- SDK 会滤除相邻完整相同的 IMU 载荷。回调计数是被交付的新样本数，不是所有线上帧数；必要时结合 Transport 帧数和固件 UART 计数区分重复包、丢包及采样变慢。 +- `mcu_timestamp_us` 表示设备侧时间；`timestamp_is_64bit=True` 是协商后的原生 V2，legacy 展开时间仍有歧义。保存 `boot_session_id`、`timestamp_valid`、`timestamp_discontinuity`，会话首帧的 discontinuity 与运行中意外跳变分别处理。 +- `host_time` 与 callback 中的 `time.monotonic()` 属于主机单调时钟；当前解码位于分发路径，不能把它当作精确 UART 到达或硬件采样时刻。它们可反映应用接收/分发节奏，不能和设备启动时基直接相减声称端到端延迟，也不证明多设备同步。 +- 电机 `s.raw.status_timestamp_ms` 是设备最近一次实际状态反馈时刻；结合 `monitor_version`/有效标志检查其推进，避免“主机持续收到旧状态”假稳定。`monitor_version=0` 表示扩展缺失，零填充字段不能当作已测零故障。 +- `control_stats.actual_hz` 的窗口由统计查询推进；查询时控制已停止会返回 0。运动段应在 `controller.stop()` 前取末次统计，并记录 `sample_ms`。还应使用 `loop_count` 差分与窗口时长独立交叉核对，避免启动/停止部分污染稳态窗口。 +- 当前 SDK 文档将 `timeout_count` 描述为 reserved，但上述候选固件已在 host watchdog 首次进入 T1 时递增。按已核实固件语义解释，不把旧固件的零值当作无超时证明。当前固件控制周期是 500 Hz；SDK 状态驱动的目标提交通常是 100 Hz，这是不同指标。 + +## 6. 量化记录与判据 + +| 记录域 | 字段/计算 | +| --- | --- | +| 区段 | run_id、A/B/A 次序、设定频率、预热/测量起止、墙钟时间与单调时长、诊断查询时点、完整参数 | +| Motor | host_time、callback 单调时间、monitor_version、status_timestamp_ms、actual/target pos/vel/torque、温度、status、stop_reason、fault/latched_fault_code；速率、帧间隔 p50/p95/p99/max、反馈新鲜度 | +| IMU | host_time、callback 单调时间、mcu_timestamp_us、seq、boot_session_id、所有 timestamp 标志、valid_flag、accel m/s²、gyro rad/s、mag μT、温度；新样本率、设备时间间隔、有效位缺失率 | +| 固件控制 | running/mode、target_hz/actual_hz、period_ms/sample_ms、target_seq/applied_seq、loop/error/deadline_miss/timeout 差分、last_error、target_age_ms、target_update_hz | +| 固件 UART | tx_bytes_ok/tx_calls_ok、tx_fail_timeout/tx_fail_other、rx_bytes/rx_errors/rx_rearms/rx_overflow、debug_tx_bytes/log_dropped 的差分 | +| SDK Transport | bytes/frames、crc_errors/resync_bytes/parser_overflow_bytes、queue_dropped/queue_high_water、callback_max_us、ack_timeouts/retries/callback_exceptions；采集自身丢样另计 | +| 运动质量 | 每程起止时间、目标和实际位置、末端误差、完成率、速度均值/标准差/峰峰值、反向帧、力矩/温度峰值；保持阶段滑移、外部力或位移测量 | + +所有累计计数按真实位宽做无符号差分，检测设备重启后开启新段，不跨重启直接相减。`callback_max_us` 等历史最大值和高水位不能直接当作区段差值。保存原始时间序列，摘要同时报告分位数和最坏间隔，不能只报告平均帧率。 + +正常稳定段的硬失败包括：非预期运动/夹持失效/复位/失能、控制器 FAULT、控制错误或 host timeout 增量、UART 发送失败或 RX 溢出、CRC/parser 错误或分发/采集队列丢样、运行中无法解释的时间回退/会话切换、反馈长期不推进、应有流消失。旧故障与新故障分开；在初始故障来源未解释前不能直接继续运动。 + +`deadline_miss_count` 增加说明发生了周期未按期完成，应列为实时性异常并保留上下文，不能因轨迹最终到位忽略。频率偏差、p99/最大间隔、deadline miss 比例、位置误差、速度纹波、力矩/温升、最大无数据时间、停止时间和长期完成率的产品验收上限必须在运行前由需求/台架负责人给定。尚无阈值时报告 A/B/A 原始值与差异，结论为“已测量、验收阈值待定”，不能自行指定通用百分比后宣称不影响电机。 + +即便应用数据完全有效，静态加速度幅值、陀螺零偏、磁场合理性与轴向仍须独立验证。电机通断电、不同力矩和姿态会影响磁力计；不把磁航向当作电机附近的绝对真值。 + +## 7. 源码可复用测试与覆盖边界 + +| 测试 | 证明范围 | +| --- | --- | +| [test_follower_imu_pty.cpp](../cpp/tests/test_follower_imu_pty.cpp) | motor-only 默认、旧固件拒绝不改流、双回调、非法频率和校准协议流程 | +| [test_imu_motor_concurrency.cpp](../cpp/tests/test_imu_motor_concurrency.cpp) | 两类控制器保留混合流；阻塞/持续慢用户回调与控制隔离，真实断流仍零输出；旧用户样本保持接收时间；控制队列溢出锁存与重启拒绝；故障后紧跟健康帧仍锁存；关闭生命周期。PTY 不代表真实调度/运动验收 | +| [test_imu_time.cpp](../cpp/tests/test_imu_time.cpp)、[test_imu_time.py](../python/tests/test_imu_time.py) | 时间协商、回绕/歧义、会话、重复样本等协议与绑定行为 | +| [test_transport_concurrent_write.cpp](../cpp/tests/test_transport_concurrent_write.cpp)、[test_dispatch_decoupling.py](../python/tests/test_dispatch_decoupling.py)、[test_port_release.py](../python/tests/test_port_release.py) | 并发写入、读/分发分离、端口释放相关行为 | +| [control_ripple.py](../python/examples/control_ripple.py) | 实机速度纹波统计可供复用；现有脚本包含实际运动且无 IMU A/B 参数，不能直接等同本文完整验收 | + +固件仓库的 `tools/slave_imu_test/run.sh` 运行生产 service/stream/寄存器/启动代码的主机替身测试，`tools/timebase_test/run.sh` 运行时间、双角色协议及主爪流回归。替身中的 RTOS、SPI、CAN、Flash 不覆盖真实抢占、电气故障或电机运动。对固件建立独立源码身份和结果记录，不能将 SDK 测试 SHA 当作固件验证身份。 + +## 8. 软件验证命令 + +以下从本 SDK 仓库根目录执行，使用已配置好依赖的测试环境。CMake 与现有 CI 同样构建核心、Python、示例和测试；生成的本地扩展不代表已安装到其他业务环境。不要在运行硬件控制的进程旁并行运行会占用相同端口的测试。 + +```bash +cmake -S . -B build -G Ninja \ + -DTACCAP_BUILD_TESTS=ON -DTACCAP_BUILD_EXAMPLES=ON \ + -DTACCAP_BUILD_PYTHON=ON \ + -Dpybind11_DIR="$(python -m pybind11 --cmakedir)" +cmake --build build + +# 有关功能先单独检查；随后执行完整软件回归。 +./build/cpp/tests/taccap_unit_tests \ + --gtest_filter='FollowerImu.*:ImuMotorConcurrency.*' +./build/cpp/tests/taccap_unit_tests +env -u PYTHONPATH python -m pytest python/tests -q + +# 或使用 CTest 运行已注册的 C++ 测试。 +ctest --test-dir build --output-on-failure +``` + +若不能覆盖源码目录中正在使用的 native，可将上述配置增加 `-DSKBUILD=ON`，使扩展保留在独立构建目录 `build/python/`；随后用 `TACCAP_TEST_NATIVE_DIR="$(pwd)/build/python" python -m pytest python/tests -q`。该测试入口严格验证实际 native 路径，无效目录立即拒绝。仍须用 `ldd` 或进程模块映射确认 core 也来自同一构建，不要用这个入口绕过来源核验；默认未设置时继续要求源码目录的 native。 + +若另有对应固件仓库 checkout，从该仓库根目录执行，不依赖某台开发机的绝对路径： + +```bash +git branch --show-current +git rev-parse HEAD +git status --short +bash tools/slave_imu_test/run.sh +bash tools/timebase_test/run.sh +``` + +将实际命令、退出码、原始日志、通过/失败/跳过项和环境写入本次运行证据。软件 PASS 与硬件 PASS 分开，CI 未查询时不声称 CI 通过；完整性结论必须列出尚未具备的电机型号、负载、故障场景和长期运行覆盖。 diff --git a/docs/UART_RELIABILITY.md b/docs/UART_RELIABILITY.md new file mode 100644 index 0000000..f4e48b8 --- /dev/null +++ b/docs/UART_RELIABILITY.md @@ -0,0 +1,77 @@ +# UART reliability candidate (2026-10-28) + +This source change addresses reproducible code risks; it does **not** establish +that the EL05 short frames are fixed. No firmware flash, live-port access, camera +access or host installation is part of the offline verification. + +## Transport + +- Reserve pending ACK sequence slots until the requesting call releases them, + including SameSeq retry delays. No-ACK allocation skips reserved slots. +- Match echoed command as well as sequence; keep legacy cmd=0, one-byte ACK/NACK + support. A mismatched response cannot consume a waiting request. +- Clear inherited INPCK/IGNPAR in raw serial setup. Existing full-frame write + locking, one serial reader, retries and independent dispatch remain. +- The wire has only 8 sequence bits. An arbitrarily late response with the same + sequence **and command** after slot reuse remains indistinguishable without a + protocol transaction/session identifier. This patch does not claim otherwise. +- Target writes are counted after successful full writes, under the writer lock; + four target commands (position, velocity, torque, impedance) share the counter. + ACK retries count as wire attempts. These are not MCU acceptance counts. + +## Explicit observations (C++ and Python) + +Existing `diagnostics.uart_stats()` sends an empty 0x54 request and retains the +44-byte struct. `diagnostics.uart_rx_errors()` sends payload `01` and requires a +72-byte reply. It returns overrun, framing, noise, parity, receiver_timeout, +last_error (HAL bitmask) and last_error_ms (32-bit boot milliseconds). Missing +support is an error, never zero-filled. Multiple error bits can count in one event. +Successful HAL TX counts are not pin-level proof and cannot exclude firmware. + +`motor.target_writes()` returns local count, last_write_ns (host steady clock), +last_gap_us and max_gap_us. It sends nothing. Gaps include deliberate pauses and +are cumulative for the Transport lifetime; start a new baseline for a new run. + +`motor.delivery_snapshot()` makes one explicit 0x51 control-stat query and returns +`before`, `after` local write snapshots plus `firmware` (target_seq, applied_seq, +loop/error counts etc.). Call from a monitoring thread, never from a controller +or a serial callback. No automatic polling or change to the motor stop policy. + +For snapshots a and b on the same boot/connection: + +```python +# g is the application's already-open follower. No discovery/open in this example. +a = g.motor.delivery_snapshot() +# ... application continues its normal control workload ... +b = g.motor.delivery_snapshot() +host_min = max(0, b.before.count - a.after.count) +host_max = b.after.count - a.before.count +accepted = (b.firmware.target_seq - a.firmware.target_seq) & 0xffffffff +print(f"Host write window: {host_min}..{host_max}, MCU accepted: {accepted}") +print(f"Host max write gap: {b.after.max_gap_us / 1000:.3f} ms") +``` + +This range brackets **query overlap**, not arbitrary bytes already buffered in +the OS/bridge/MCU. It cannot prove exact loss for a short window. Invalidate the +baseline after restart, counter reset, reconnect, other command producers or +control-mode changes. Do not equate target_seq-applied_seq with packet loss: +multiple targets can legitimately be replaced before a 500 Hz controller step. + +Feedback-paced controllers remain feedback-paced. A status frame does not prove +an idle interval: IMU/ACK frames may immediately follow it. Average 100 Hz is not +a guarantee of 10 ms spacing. No stale target replay is introduced. + +## Validation and deployment + +Run the PTY reliability cases plus existing transport, controller, IMU and OTA +regressions; compile the Python bindings. PTYs cannot reproduce UART electrical +errors, physical FIFO overrun, bridge loss, USB contention or camera throughput. +Build with `-DSKBUILD=ON` into a separate directory to avoid replacing an SDK +library used by a running test. New Python sources and native libraries must come +from the same build. Do not install or replace a running test's module. + +Use the firmware's optional error classification in the next matched hardware +run. Keep logs off; retain raw host input and, if authorized, follower-only +usbmon data. Test main/modified cross combinations separately. This patch does +not change camera capture/Rectify performance or claim to recover the observed +101 fps to the configured 120 fps. diff --git a/firmware/README.md b/firmware/README.md index fe7dce4..9a896d1 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -5,8 +5,8 @@ part of this SDK. | Image | Role | Version | Protocol | Size | CRC32 | | --- | --- | --- | --- | --- | --- | -| `tc-gu-01-master-1.2.6.bin` | leader (SN ends **`m`**) | **1.2.6** | V2.6 | 118,300 B | `0x350393b5` | -| `tc-gu-01-slave-1.2.16.bin` | follower (SN ends **`s`**) | **1.2.16** | V2.8 | 167,544 B | `0x4a8cd360` | +| `tc-gu-01-master-1.2.7.bin` | leader (SN ends **`m`**) | **1.2.7** | V2.9 | 122,848 B | `0x19e881dd` | +| `tc-gu-01-slave-1.2.17.bin` | follower (SN ends **`s`**) | **1.2.17** | V2.9 | 180,100 B | `0xf019df36` | The follower's RobStride motor runs its own firmware; its images are listed under `motors` in the manifest (see [Motor firmware](#motor-firmware)): @@ -23,6 +23,11 @@ Leader and follower firmware are **versioned independently**; different numbers on the two halves of a pair are normal. The SDK refuses to drive a follower below **1.2.5**. +> **1.2.7 / 1.2.17 change the IMU/encoder `timestamp` timebase** (DWT/250, wrapping every +> 17.18 s → TIM2 1 MHz, wrapping every 71 min 35 s). This SDK handles both; older host +> software that unwraps with the 17.18 s period will produce wrong times. Upgrade host +> software together with the firmware. Protocol: tc-gu-01 `docs/PROTOCOL.md` V2.9. + ## Flashing ```bash @@ -53,7 +58,7 @@ To verify an image file before flashing: ```bash python -c "from xense.taccap import crc32_iso_hdlc -print(hex(crc32_iso_hdlc(open('firmware/tc-gu-01-master-1.2.6.bin','rb').read())))" +print(hex(crc32_iso_hdlc(open('firmware/tc-gu-01-master-1.2.7.bin','rb').read())))" # -> 0x350393b5 ``` diff --git a/firmware/manifest.json b/firmware/manifest.json index cff837c..988e58b 100644 --- a/firmware/manifest.json +++ b/firmware/manifest.json @@ -1,28 +1,28 @@ { "images": { "master": { - "file": "tc-gu-01-master-1.2.6.bin", - "version": "1.2.6", - "source_commit": "1336ebb", - "size_bytes": 118300, - "crc32": "0x350393b5", + "file": "tc-gu-01-master-1.2.7.bin", + "version": "1.2.7", + "source_commit": "8c059e9", + "size_bytes": 122848, + "crc32": "0x19e881dd", "sn_suffix": "m", - "protocol": "V2.6", + "protocol": "V2.9", "build": "local", - "toolchain": "arm-none-eabi-gcc (15:13.2.rel1-2) 13.2.1 20231009", - "note": "Leader line. Same fix as follower 1.2.11 in the shared receive path: the control UART (3 Mbaud, one interrupt per byte) stopped receiving for good after a single overrun, because the error callback never restarted reception -- the stream kept flowing while every command went unanswered. Reception is now restarted, and GET_UART_STATS reports rx_errors / rx_rearms. Verified on 0115m and 0116m (upgraded from 1.2.2): 15 min each of a 100 Hz encoder+IMU stream with a 100 Hz acknowledged command, ~87,000 commands, zero failures. AFTER FLASHING, unplug and replug the leader's USB cable." + "toolchain": "arm-none-eabi-gcc (15:10.3-2021.07-4) 10.3.1 20210621", + "note": "Leader time-shared exchange (STREAM_MODE_POLLED + LEADER_EXCHANGE 0x5E): the leader queues uplink records and only answers, so host and leader never transmit at the same time -- an uplink frame being sent while a host byte arrives loses the bytes in flight. Verified on the taccap-collector backpack (0029m/0030m): 0 bad uplink frames under full host load with ~20 Hz LED commands vs 17/7208 pushing on 1.2.6. HOST-BREAKING: IMU/encoder timestamp is now TIM2 1 MHz (low 32 bits, wraps every 71 min 35 s), no longer DWT/250 (17.18 s); upgrade host software together. Known: after a power-up the leader can stop answering ~0.5 s after the first stream start until the port is reopened (also on 1.2.6). AFTER FLASHING, unplug and replug the leader's USB cable." }, "slave": { - "file": "tc-gu-01-slave-1.2.16.bin", - "version": "1.2.16", - "source_commit": "a7dc07f", - "size_bytes": 167544, - "crc32": "0x4a8cd360", + "file": "tc-gu-01-slave-1.2.17.bin", + "version": "1.2.17", + "source_commit": "8c059e9", + "size_bytes": 180100, + "crc32": "0xf019df36", "sn_suffix": "s", - "protocol": "V2.8 + motion envelope", + "protocol": "V2.9 + motion envelope", "build": "local", - "toolchain": "arm-none-eabi-gcc (15:13.2.rel1-2) 13.2.1 20231009", - "note": "Stops decoding the motor's echo frames as status. A RobStride motor takes an MIT motion frame whose byte 0 equals its own CAN ID for another command and answers on 0xFD with the frame's first four bytes and zeros in bytes 4-7; byte 0 is the target position's high byte, and the factory ID 127 (0x7F) is -0.098..0 rad -- a LEFT-hand RS00's closed end. 1.2.15 decoded each echo as status (temperature 0, torque -t_max, position clamped to the far end), the envelope clamp then dragged the target toward the open end and the jaw was thrown open: force-position never closed to 0.0 on 0089s. The real fix is the CAN ID (RS00 followers: left 17, right 18, written by the factory config tool); this image is the backstop. Carries 1.2.15 (RS00 open direction from the SN's side). Verified on 0089s (left) / 0094s (right). AFTER FLASHING, unplug the 24 V power cable for ~2 s and plug it back; the USB cable can stay in." + "toolchain": "arm-none-eabi-gcc (15:10.3-2021.07-4) 10.3.1 20210621", + "note": "Follower time-shared exchange (SLAVE_EXCHANGE 0x5D, queue 32), optional 1-100 Hz IMU with accel/gyro calibration and magnetometer sessions, uplink collision detect-and-resend, cold-start homing order fix. HOST-BREAKING: IMU timestamp is now TIM2 1 MHz (wraps every 71 min 35 s). Carries 1.2.16. AFTER FLASHING, unplug the 24 V power cable for ~2 s and plug it back; the USB cable can stay in." } }, "motors": { diff --git a/firmware/tc-gu-01-master-1.2.6.bin b/firmware/tc-gu-01-master-1.2.6.bin deleted file mode 100755 index 73d812f..0000000 Binary files a/firmware/tc-gu-01-master-1.2.6.bin and /dev/null differ diff --git a/firmware/tc-gu-01-master-1.2.7.bin b/firmware/tc-gu-01-master-1.2.7.bin new file mode 100755 index 0000000..9e3cf53 Binary files /dev/null and b/firmware/tc-gu-01-master-1.2.7.bin differ diff --git a/firmware/tc-gu-01-slave-1.2.16.bin b/firmware/tc-gu-01-slave-1.2.16.bin deleted file mode 100755 index 033f7f8..0000000 Binary files a/firmware/tc-gu-01-slave-1.2.16.bin and /dev/null differ diff --git a/firmware/tc-gu-01-slave-1.2.17.bin b/firmware/tc-gu-01-slave-1.2.17.bin new file mode 100755 index 0000000..a7e732d Binary files /dev/null and b/firmware/tc-gu-01-slave-1.2.17.bin differ diff --git a/python/bindings/bind_gripper.cpp b/python/bindings/bind_gripper.cpp index f8c907b..f86377e 100644 --- a/python/bindings/bind_gripper.cpp +++ b/python/bindings/bind_gripper.cpp @@ -122,34 +122,77 @@ void bind_gripper(py::module_& m) { " fw_sn=" + e.firmware_sn + (e.in_use ? " IN USE" : "") + ")"; }); + // Discovery opens and probes every serial port (~100+ ms per free device, + // up to the ACK timeout on a silent one). Drop the GIL so a scan on a + // background thread does not freeze the caller's other Python threads + // (e.g. a GUI pushing 100 Hz targets). Results are converted after the + // GIL is re-acquired. m.def("scan_grippers", &discovery::scan_all, + py::call_guard(), "Enumerate every gripper currently plugged in.\n\n" "Opens a transient link to each MCU and probes it for DEV_TYPE and the\n" "SN before closing it again, so a scan is not free and cannot run while\n" "another process holds the port. Returns an empty list rather than\n" "raising when nothing is attached."); m.def("find_one", &discovery::find_one, + py::call_guard(), "The single attached gripper, whatever its side or role.\n\n" "Raises IoError when none is attached, and again when more than one is --\n" "pick with find_left()/find_right() or find_leader()/find_follower()."); m.def("find_left", &discovery::find_left, + py::call_guard(), "The attached gripper whose firmware reports side Left.\n\n" "Raises IoError when none matches; a unit whose firmware answered neither\n" "GetSn nor GetDevType has side Unknown and never matches."); m.def("find_right", &discovery::find_right, + py::call_guard(), "The attached gripper whose firmware reports side Right.\n\n" "Raises IoError when none matches; a unit whose firmware answered neither\n" "GetSn nor GetDevType has side Unknown and never matches."); m.def("find_leader", &discovery::find_leader, + py::call_guard(), "The attached gripper whose SN patch suffix is 'm' (Master).\n\n" "Raises IoError when none matches; a legacy or unburned SN reports role\n" "Unknown and never matches."); m.def("find_follower", &discovery::find_follower, + py::call_guard(), "The attached gripper whose SN patch suffix is 's' (Slave).\n\n" "Raises IoError when none matches; a legacy or unburned SN reports role\n" "Unknown and never matches."); // ---- LeaderGripper -------------------------------------------------- + py::class_(m, "LeaderExchangeStats", + "Counters of LeaderGripper's half-duplex exchange since start_exchange().") + .def_readonly("exchanges", &LeaderExchangeStats::exchanges) + .def_readonly("continuation_exchanges", &LeaderExchangeStats::continuation_exchanges, + "Exchanges sent at once because the firmware reported a backlog.") + .def_readonly("timeouts", &LeaderExchangeStats::timeouts, + "Exchanges without a reply even after same-seq retries.") + .def_readonly("nacks", &LeaderExchangeStats::nacks) + .def_readonly("malformed", &LeaderExchangeStats::malformed) + .def_readonly("not_polled", &LeaderExchangeStats::not_polled, + "Replies saying the firmware left the exchange (1 s lease expired, stream restarted or reboot).") + .def_readonly("rearms", &LeaderExchangeStats::rearms, + "Times the SDK re-sent the polled StartStream after a not_polled reply.") + .def_readonly("rearm_failures", &LeaderExchangeStats::rearm_failures) + .def_readonly("records", &LeaderExchangeStats::records) + .def_readonly("records_dropped", &LeaderExchangeStats::records_dropped, + "Records the firmware queue overflowed before they were fetched.") + .def_readonly("imu_samples", &LeaderExchangeStats::imu_samples) + .def_readonly("encoder_samples", &LeaderExchangeStats::encoder_samples) + .def_readonly("key_events", &LeaderExchangeStats::key_events) + .def_readonly("sensor_errors", &LeaderExchangeStats::sensor_errors) + .def_readonly("other_records", &LeaderExchangeStats::other_records) + .def_readonly("last_rtt_us", &LeaderExchangeStats::last_rtt_us) + .def_readonly("max_rtt_us", &LeaderExchangeStats::max_rtt_us) + .def("__repr__", [](const LeaderExchangeStats& s) { + return "LeaderExchangeStats(exchanges=" + std::to_string(s.exchanges) + + ", records=" + std::to_string(s.records) + + ", dropped=" + std::to_string(s.records_dropped) + + ", timeouts=" + std::to_string(s.timeouts) + + ", max_rtt_us=" + std::to_string(s.max_rtt_us) + ")"; + }); + py::class_(m, "LeaderGripper", "One TacCap-Gripper leader (teleop master end).\n\n" "Owns the MCU control link plus IMU, encoder, button, LED and -- only when\n" @@ -232,11 +275,33 @@ void bind_gripper(py::module_& m) { "Raises IoError when none is attached or when more than one is; pass an\n" "explicit mcu_device from find_leader() in that case. Cameras stay\n" "closed -- an external service owns the wrist V4L2 device.") - .def("start_streaming", [](LeaderGripper& self, unsigned imu_hz, unsigned enc_hz) { + .def("start_streaming", [](LeaderGripper& self, unsigned imu_hz, unsigned enc_hz, bool v2) { + py::gil_scoped_release gil; + self.start_streaming(imu_hz, enc_hz, v2); + }, py::arg("imu_hz") = 100u, py::arg("encoder_hz") = 100u, py::arg("imu_v2_time") = true, + "Start the MCU sensor stream.\n\nimu_v2_time=True (default) streams IMU V2 with 64-bit capture time when the firmware supports it; False keeps the legacy 28-byte IMU frame, as SDK <= 0.4.4 did. A pushed stream loses a frame whenever a host write overlaps it and longer frames are hit more often, so with frequent queries False loses fewer IMU samples; start_exchange() avoids the overlap entirely.\n\nA rate of 0 turns that source OFF (its source_mask bit is cleared). Passing 0 used to stream the source at the firmware's 100 Hz default instead; raises IoError(EINVAL) if every rate is 0.\n\nThe firmware divides a 1 kHz tick by an integer, so only divisors of 1000 arrive at the requested rate -- 300 Hz becomes 333 Hz, 150 Hz becomes 167 Hz, and anything above 1000 Hz collapses to 100 Hz. Motor status is additionally capped at 100 Hz. None of this is NACKed by the firmware, so the SDK logs a warning when it applies.") + .def("start_exchange", [](LeaderGripper& self, unsigned imu_hz, unsigned enc_hz, + unsigned exchange_hz, bool fallback) { py::gil_scoped_release gil; - self.start_streaming(imu_hz, enc_hz); - }, py::arg("imu_hz") = 100u, py::arg("encoder_hz") = 100u, - "Start the MCU sensor stream.\n\nA rate of 0 turns that source OFF (its source_mask bit is cleared). Passing 0 used to stream the source at the firmware's 100 Hz default instead; raises IoError(EINVAL) if every rate is 0.\n\nThe firmware divides a 1 kHz tick by an integer, so only divisors of 1000 arrive at the requested rate -- 300 Hz becomes 333 Hz, 150 Hz becomes 167 Hz, and anything above 1000 Hz collapses to 100 Hz. Motor status is additionally capped at 100 Hz. None of this is NACKed by the firmware, so the SDK logs a warning when it applies.") + return self.start_exchange(imu_hz, enc_hz, exchange_hz, fallback); + }, py::arg("imu_hz") = 100u, py::arg("encoder_hz") = 100u, py::arg("exchange_hz") = 0u, + py::arg("fallback_to_streaming") = true, + "Half-duplex alternative to start_streaming() (firmware with 0x5E).\n\n" + "The leader stops pushing frames and holds IMU, encoder, key and sensor-error\n" + "frames; the SDK fetches them once per 1/exchange_hz s and delivers them to the\n" + "same subscribers. The host never writes while the leader is answering, which\n" + "removes the overlap that loses uplink bytes. exchange_hz=0 means\n" + "max(imu_hz, encoder_hz, 1) capped at 200 (explicit values up to 500); pass 2x the data rate (e.g. 200) to avoid a sample\n" + "occasionally waiting one extra period. Samples from one reply share its arrival\n" + "time as host_time; use mcu_timestamp_us for the sampling instant. If the host stalls\n" + "for more than 1 s the leader returns to pushing on its own and the SDK re-enters\n" + "the exchange afterwards. Returns True when the exchange runs; on firmware\n" + "without 0x5E it falls back to start_streaming() and returns False, or raises\n" + "ProtocolError when fallback_to_streaming=False. stop_streaming() ends it.") + .def_property_readonly("is_exchanging", &LeaderGripper::is_exchanging, + "True between a successful start_exchange() and stop_streaming().") + .def("exchange_stats", &LeaderGripper::exchange_stats, + "LeaderExchangeStats of the running exchange (zeros when not exchanging).") .def("stop_streaming", [](LeaderGripper& self) { py::gil_scoped_release gil; self.stop_streaming(); @@ -371,6 +436,36 @@ void bind_gripper(py::module_& m) { "." + std::to_string(v.build) + ")"; }); + py::class_(m, "ExchangeStats", + "Counters of FollowerGripper's half-duplex exchange since start_exchange().") + .def_readonly("exchanges", &ExchangeStats::exchanges) + .def_readonly("telemetry_exchanges", &ExchangeStats::telemetry_exchanges) + .def_readonly("target_exchanges", &ExchangeStats::target_exchanges) + .def_readonly("timeouts", &ExchangeStats::timeouts, + "Exchanges without a reply; a carried target is resent.") + .def_readonly("nacks", &ExchangeStats::nacks) + .def_readonly("malformed", &ExchangeStats::malformed) + .def_readonly("targets_submitted", &ExchangeStats::targets_submitted) + .def_readonly("targets_superseded", &ExchangeStats::targets_superseded, + "Targets replaced by a newer one before they went out.") + .def_readonly("targets_resent", &ExchangeStats::targets_resent) + .def_readonly("targets_rejected", &ExchangeStats::targets_rejected) + .def_readonly("last_target_result", &ExchangeStats::last_target_result, + "Raw ErrorCode byte of the latest target reply; 0xFF before any.") + .def_readonly("motor_samples", &ExchangeStats::motor_samples) + .def_readonly("imu_samples", &ExchangeStats::imu_samples) + .def_readonly("imu_dropped", &ExchangeStats::imu_dropped, + "IMU samples the firmware queue overflowed before they were fetched.") + .def_readonly("last_rtt_us", &ExchangeStats::last_rtt_us) + .def_readonly("max_rtt_us", &ExchangeStats::max_rtt_us) + .def("__repr__", [](const ExchangeStats& s) { + return "ExchangeStats(exchanges=" + std::to_string(s.exchanges) + + ", timeouts=" + std::to_string(s.timeouts) + + ", targets=" + std::to_string(s.target_exchanges) + + ", rejected=" + std::to_string(s.targets_rejected) + + ", max_rtt_us=" + std::to_string(s.max_rtt_us) + ")"; + }); + py::class_(m, "FollowerGripper", "One TacCap-Gripper follower (robot-side executor).\n\n" "The leader's sensor surface plus a Motor driving the FDCAN-attached\n" @@ -474,22 +569,34 @@ void bind_gripper(py::module_& m) { "explicit mcu_device from find_follower() in that case. It does not\n" "check that the hardware really is a follower -- see the class docstring.\n" "Cameras stay closed.") - .def("start_streaming", [](FollowerGripper& self, unsigned motor_hz) { + .def("start_streaming", [](FollowerGripper& self, unsigned motor_hz, unsigned imu_hz) { py::gil_scoped_release gil; - self.start_streaming(motor_hz); - }, py::arg("motor_hz") = 100u, - "Start the motor-status stream.\n\n" - "Motor status is the ONLY source a follower streams -- the firmware\n" - "compiles IMU, encoder and eskin streaming out on this role. This used\n" - "to take imu_hz and encoder_hz for parity with the leader; they set\n" - "their mask bits and produced nothing, and the old defaults\n" - "(imu=100, encoder=100, motor=0) meant a bare start_streaming() call\n" - "started a stream carrying NOTHING and reported success.\n\n" - "motor_hz=0 raises IoError(EINVAL) rather than starting an empty\n" + self.start_streaming(motor_hz, imu_hz); + }, py::arg("motor_hz") = 100u, py::arg("imu_hz") = 0u, + "Start motor status, optionally IMU (default off).\n\n" + "imu_hz=1..100 requires capable follower firmware; an IMU read probes\n" + "support before changing the stream. Old firmware raises ProtocolError.\n" + "Stop streaming before changing rates. Both rates zero raises IoError(EINVAL).\n" "stream. The firmware caps motor status at 100 Hz and divides a 1 kHz\n" "tick by an integer, so only divisors of 1000 arrive at the requested\n" "rate. It never NACKs a rate it had to adjust, so the SDK logs a\n" "warning when that happens.") + .def("start_exchange", [](FollowerGripper& self, unsigned motor_hz, unsigned imu_hz, + unsigned exchange_hz) { + py::gil_scoped_release gil; + self.start_exchange(motor_hz, imu_hz, exchange_hz); + }, py::arg("motor_hz") = 100u, py::arg("imu_hz") = 0u, py::arg("exchange_hz") = 0u, + "Half-duplex alternative to start_streaming() (firmware with 0x5D).\n\n" + "The follower stops pushing frames; the SDK polls it once per\n" + "1/exchange_hz s and delivers motor status / IMU to the usual\n" + "subscribers. motor.submit_*() targets ride on the exchange. Every\n" + "request on this link takes turns, so host and follower never transmit\n" + "at the same time. exchange_hz=0 means max(motor_hz, imu_hz). End it with\n" + "stop_streaming(). Old firmware raises ProtocolError.") + .def_property_readonly("is_exchanging", &FollowerGripper::is_exchanging, + "True between start_exchange() and stop_streaming().") + .def("exchange_stats", &FollowerGripper::exchange_stats, + "ExchangeStats of the running exchange (zeros when not exchanging).") .def("stop_streaming", [](FollowerGripper& self) { py::gil_scoped_release gil; self.stop_streaming(); @@ -497,8 +604,8 @@ void bind_gripper(py::module_& m) { "Best-effort: a no-op when no stream was started, and it returns even\n" "when the MCU never ACKs the StopStream.") .def_property_readonly("imu", [](FollowerGripper& g) -> IMU& { return g.imu(); }, py::return_value_policy::reference_internal, - "IMU component. Present on both roles, but a follower does not stream\n" - "it -- see start_streaming().") + "IMU component; follower support requires capable firmware and explicit\n" + "imu_hz opt-in -- see start_streaming().") .def_property_readonly("encoder", [](FollowerGripper& g) -> Encoder& { return g.encoder(); }, py::return_value_policy::reference_internal, "Encoder component. Present on both roles, but a follower does not\n" "stream it -- see start_streaming().") diff --git a/python/bindings/bind_motor.cpp b/python/bindings/bind_motor.cpp index 58405d2..ed86e0a 100644 --- a/python/bindings/bind_motor.cpp +++ b/python/bindings/bind_motor.cpp @@ -43,10 +43,9 @@ void bind_motor(py::module_& m) { py::class_(m, "UartStats", "Free-running firmware UART counters since MCU boot (Cmd 0x54).\n\n" "Take a reading before and after a measurement window and subtract.\n" - "tx_bytes_ok / tx_calls_ok count only what the firmware's transmit call\n" - "accepted, i.e. what reached the MCU's transmit register -- compare them\n" - "against what the host decoded to tell 'the MCU never sent it' apart from\n" - "'it was lost after leaving the MCU'.\n\n" + "tx_bytes_ok / tx_calls_ok count successful HAL transmit calls, not\n" + "pin-level capture. Compare aligned windows; a difference alone cannot\n" + "assign the fault to MCU firmware, bridge, USB or TTY.\n\n" "log_dropped reads 0 on firmware 1.1.3, which had no such field; that is\n" "not distinguishable from a genuine zero.") .def_readonly("tx_bytes_ok", &protocol::UartStats::tx_bytes_ok) @@ -117,9 +116,32 @@ void bind_motor(py::module_& m) { m.attr("LOG_OUTPUT_NONE") = py::int_(protocol::LogOutput::None); m.attr("LOG_OUTPUT_UART") = py::int_(protocol::LogOutput::Uart); + py::class_(m, "UartRxErrors") + .def_readonly("overrun", &protocol::UartRxErrors::overrun) + .def_readonly("framing", &protocol::UartRxErrors::framing) + .def_readonly("noise", &protocol::UartRxErrors::noise) + .def_readonly("parity", &protocol::UartRxErrors::parity) + .def_readonly("receiver_timeout", &protocol::UartRxErrors::receiver_timeout) + .def_readonly("last_error", &protocol::UartRxErrors::last_error) + .def_readonly("last_error_ms", &protocol::UartRxErrors::last_error_ms); + py::class_(m, "MotorTargetWrites") + .def_readonly("count", &bus::Transport::TargetWrites::count) + .def_readonly("last_write_ns", &bus::Transport::TargetWrites::last_write_ns) + .def_readonly("last_gap_us", &bus::Transport::TargetWrites::last_gap_us) + .def_readonly("max_gap_us", &bus::Transport::TargetWrites::max_gap_us); + py::class_(m, "MotorDeliverySnapshot") + .def_readonly("before", &MotorDeliverySnapshot::before) + .def_readonly("after", &MotorDeliverySnapshot::after) + .def_readonly("firmware", &MotorDeliverySnapshot::firmware); + py::class_(m, "Diagnostics", "Firmware UART counters and log control. Available on both gripper\n" "roles; needs firmware 1.1.3 (counters) / 1.1.4 (log control).") + .def("uart_rx_errors", [](Diagnostics& self, unsigned timeout_ms) { + py::gil_scoped_release release; + return self.uart_rx_errors(std::chrono::milliseconds(timeout_ms)); + }, py::arg("timeout_ms") = 100, + "Explicit extended UART error query; unsupported firmware raises ProtocolError.") .def("uart_stats", [](Diagnostics& self, unsigned timeout_ms) { py::gil_scoped_release g; return self.uart_stats(std::chrono::milliseconds(timeout_ms)); @@ -824,6 +846,13 @@ void bind_motor(py::module_& m) { "get_private_param().\n\n" "Writing 0x700B also rewrites the persisted startup limit torque, so a\n" "one-off tweak silently becomes the new boot default.") + .def("target_writes", &Motor::target_writes, py::call_guard(), + "Local counters only; does not query or control the motor.") + .def("delivery_snapshot", [](Motor& self, unsigned timeout_ms) { + py::gil_scoped_release release; + return self.delivery_snapshot(std::chrono::milliseconds(timeout_ms)); + }, py::arg("timeout_ms") = 100, + "One control-stats query bracketed by host target-write counters.") .def("control_stats", [](Motor& self, unsigned timeout_ms) { py::gil_scoped_release g; return self.control_stats(std::chrono::milliseconds(timeout_ms)); diff --git a/python/bindings/bind_sensors.cpp b/python/bindings/bind_sensors.cpp index 6cb96b9..bb85f08 100644 --- a/python/bindings/bind_sensors.cpp +++ b/python/bindings/bind_sensors.cpp @@ -25,7 +25,15 @@ void bind_sensors(py::module_& m) { "comparable with time.monotonic(). The MCU timestamp counts from the\n" "MCU's own boot instead.") .def_readonly("mcu_timestamp_us", &ImuSample::mcu_timestamp_us, - "MCU-side capture time, microseconds since the MCU booted.") + "64-bit capture time in microseconds. Native V2 boot epoch, or legacy observed epoch.") + .def_readonly("boot_session_id", &ImuSample::boot_session_id, + "Nonzero V2 boot identity; zero on legacy firmware. Changes after a reboot.") + .def_readonly("timestamp_is_64bit", &ImuSample::timestamp_is_64bit, + "True for native V2 timestamps, false for unfolded legacy timestamps.") + .def_readonly("timestamp_valid", &ImuSample::timestamp_valid, + "False for ambiguous legacy jumps or reordered V2 timestamps.") + .def_readonly("timestamp_discontinuity", &ImuSample::timestamp_discontinuity, + "Reset time-dependent estimators when true; this is not a clock synchronization result.") .def_readonly("valid_flag", &ImuSample::valid_flag, "Bitmask of the groups this frame carries: 0x01 accel, 0x02 gyro,\n" "0x04 mag, 0x08 temperature.") @@ -124,7 +132,8 @@ void bind_sensors(py::module_& m) { return self.read_once(std::chrono::milliseconds(timeout_ms)); }, py::arg("timeout_ms") = 100, "Poll one sample and wait for the answer. Works without start_streaming().\n\n" - "Blocks up to timeout_ms with the GIL released. Raises ProtocolError when\n" + "Negotiates native 64-bit time, then reads the sample; timeout_ms applies\n" + "to each command (plus transport retries), with the GIL released. Raises ProtocolError when\n" "the firmware NACKs or answers with the wrong payload size, TimeoutError\n" "when it does not answer at all.") .def("on_data", [](IMU& self, py::function pycb) { @@ -143,6 +152,13 @@ void bind_sensors(py::module_& m) { .def("off", &IMU::off, py::arg("subscription_id"), "Cancel a subscription taken out by on_data(). Takes effect even for\n" "frames already queued for dispatch.") + .def("calibrate_accel_gyro", [](IMU& self, unsigned timeout_ms) { + py::gil_scoped_release gil; + self.calibrate_accel_gyro(std::chrono::milliseconds(timeout_ms)); + }, py::arg("timeout_ms") = 16000u, + "Calibrate, statically verify and save accel/gyro offsets (Cmd 0x25).\n" + "Keep sensor Z up and stationary; stop control and disable follower motor first.\n" + "Timeout does not cancel device work. Requires supporting firmware.") .def("set_mag_calibration", [](IMU& self, std::array hard, std::array soft, @@ -161,13 +177,14 @@ void bind_sensors(py::module_& m) { "firmware stores after MotionCal. Blocks for the ACK with the GIL\n" "released; raises ProtocolError on NACK. Also ends a session begun with\n" "start_mag_calibration(): the firmware stops its stream and clears the\n" - "flag itself.") + "flag itself. Capable followers restore the previous stream instead.") .def("start_mag_calibration", [](IMU& self, unsigned timeout_ms) { py::gil_scoped_release gil; self.start_mag_calibration(std::chrono::milliseconds(timeout_ms)); }, py::arg("timeout_ms") = 500u, - "Begin a magnetometer calibration session (Cmd 0x26, payload 1). Leader\n" - "firmware only; a follower raises ProtocolError(InvalidCmd).\n\n" + "Begin a magnetometer calibration session (Cmd 0x26, payload 1).\n" + "Requires supporting firmware; old followers NACK. On followers stop\n" + "control and disable the motor first. Session expires after 180 seconds.\n\n" "The firmware STARTS AN IMU STREAM BY ITSELF so raw samples reach\n" "on_data() while the gripper is rotated; the gripper's own\n" "start_streaming() state does not know about it. Finish with\n" @@ -178,7 +195,8 @@ void bind_sensors(py::module_& m) { }, py::arg("timeout_ms") = 500u, "Abandon a magnetometer calibration session (Cmd 0x26, payload 2): stops\n" "the stream the firmware started and clears the flag. Not needed after\n" - "set_mag_calibration(), which does both."); + "set_mag_calibration(), which does both. Capable followers restore the\n" + "previous stream instead of stopping it."); // ---- KeySample + Key (V1.4) ---------------------------------------- py::class_(m, "KeySample", diff --git a/python/bindings/module.cpp b/python/bindings/module.cpp index dffaca4..cde7453 100644 --- a/python/bindings/module.cpp +++ b/python/bindings/module.cpp @@ -103,6 +103,9 @@ PYBIND11_MODULE(_taccap_native, m) { .value("Ws2812Set", tp::Cmd::Ws2812Set) .value("Ws2812Effect", tp::Cmd::Ws2812Effect) .value("GetImu", tp::Cmd::GetImu) + .value("TimeSession", tp::Cmd::TimeSession) + .value("GetImuV2", tp::Cmd::GetImuV2) + .value("TxTrace", tp::Cmd::TxTrace) .value("GetEncoder", tp::Cmd::GetEncoder) .value("GetEskin1", tp::Cmd::GetEskin1) .value("GetEskin2", tp::Cmd::GetEskin2) @@ -320,20 +323,22 @@ PYBIND11_MODULE(_taccap_native, m) { .def_readonly("unexpected_frames", &tb::Transport::Stats::unexpected_frames) .def_readonly("callback_exceptions", &tb::Transport::Stats::callback_exceptions) // ---- Loss accounting ------------------------------------------ - // Read together these localise a rate drop: - // crc_errors / resync_bytes rising -> bytes lost before the parser - // (the host stopped draining the tty long enough to overflow - // the kernel/USB buffers). Check callback_max_us for the cause. - // queue_dropped rising -> bytes were fine, the - // subscriber could not keep up and stale frames were evicted. - // both flat, rate still low -> the firmware really is - // sending slower; nothing host-side to fix. + // Counters identify the rejecting/dropping stage, not the original + // cause. Length errors can reject bytes before CRC; raw receive trace + // and independent MCU/link evidence are needed to locate damage. .def_readonly("crc_errors", &tb::Transport::Stats::crc_errors) .def_readonly("resync_bytes", &tb::Transport::Stats::resync_bytes) .def_readonly("parser_overflow_bytes", &tb::Transport::Stats::parser_overflow_bytes) + .def_readonly("parser_no_head", &tb::Transport::Stats::parser_no_head) + .def_readonly("parser_oversize", &tb::Transport::Stats::parser_oversize) + .def_readonly("parser_short_body", &tb::Transport::Stats::parser_short_body) + .def_readonly("parser_bad_length", &tb::Transport::Stats::parser_bad_length) .def_readonly("queue_dropped", &tb::Transport::Stats::queue_dropped) .def_readonly("queue_high_water", &tb::Transport::Stats::queue_high_water) .def_readonly("callback_max_us", &tb::Transport::Stats::callback_max_us) + .def_readonly("control_queue_dropped", &tb::Transport::Stats::control_queue_dropped) + .def_readonly("control_queue_high_water", &tb::Transport::Stats::control_queue_high_water) + .def_readonly("control_queue_overflows", &tb::Transport::Stats::control_queue_overflows) .def("__repr__", [](const tb::Transport::Stats& s) { char buf[320]; std::snprintf(buf, sizeof(buf), @@ -352,6 +357,28 @@ PYBIND11_MODULE(_taccap_native, m) { }); py::class_(m, "Transport") + .def("enable_receive_trace", &tb::Transport::enable_receive_trace, + py::arg("enabled") = true) + .def("take_receive_trace", [](tb::Transport& transport) { + auto snapshot = transport.take_receive_trace(); + py::dict result; + py::list records; + for (const auto& record : snapshot.records) { + py::dict row; + row["kind"] = record.kind == tb::FrameParser::TraceRecord::Kind::Input ? "input" : "reject"; + row["reason"] = static_cast(record.reason); + row["host_time_ns"] = std::chrono::duration_cast( + record.host_time.time_since_epoch()).count(); + row["offset"] = record.offset; + row["available"] = record.available; + row["consumed"] = record.consumed; + row["bytes"] = py::bytes(reinterpret_cast(record.bytes.data()), record.size); + records.append(std::move(row)); + } + result["records"] = std::move(records); + result["overwritten_records"] = snapshot.overwritten_records; + return result; + }) .def(py::init([](const std::string& dev, uint32_t baud, tp::Address peer, diff --git a/python/examples/ota_update.py b/python/examples/ota_update.py index 24c0980..ae0d18d 100644 --- a/python/examples/ota_update.py +++ b/python/examples/ota_update.py @@ -20,7 +20,7 @@ of the update, not as troubleshooting. The released images ship in this repo under `firmware/`, and their filenames -carry the version (tc-gu-01-slave-1.2.16.bin). Prefer the role selectors below: +carry the version (tc-gu-01-slave-1.2.17.bin). Prefer the role selectors below: they read the current filename from firmware/manifest.json, so they keep working across releases, while a literal filename goes stale the next time the firmware is bumped. A name you do pass resolves against firmware/ from any @@ -43,7 +43,7 @@ # An explicit image, when you mean a specific build rather than the # current release. - python python/examples/ota_update.py tc-gu-01-master-1.2.6.bin + python python/examples/ota_update.py tc-gu-01-master-1.2.7.bin # The version sent to the firmware (it writes bank metadata and the # post-install verification log with it) is taken from manifest.json by @@ -167,7 +167,7 @@ def _gripper_role(eps) -> str: def _default_firmware_for_role(role: str) -> str: """The shipped image for a role, named by firmware/manifest.json. - The filename carries the version (tc-gu-01-slave-1.2.16.bin), so it cannot be + The filename carries the version (tc-gu-01-slave-1.2.17.bin), so it cannot be derived from the role alone — the manifest is the single place the current release's filename is written, and `file` there is what this reads. That also means the manifest is load-bearing for every role-selector and --all @@ -363,7 +363,7 @@ def _resolve_firmware(path: str) -> Optional[str]: """Find the image whether `path` is relative to the cwd or to this repo. The images ship inside this repo, but the repo is usually vendored as a - submodule of something else — so `firmware/tc-gu-01-master-1.2.6.bin`, the path + submodule of something else — so `firmware/tc-gu-01-master-1.2.7.bin`, the path our docs print because it works from the SDK root, is not the path that works from the parent repo's root. Rather than making every downstream README carry its own prefix, accept both: the literal path first, then the @@ -414,7 +414,7 @@ def _identify_image(fw_bytes: bytes): """Look up the CRC32 in firmware/manifest.json; return (role, meta) or (None, None). **Identify by content, not by file name.** Released image file names carry - the version (tc-gu-01-slave-1.2.16.bin), but file names can change — copy + the version (tc-gu-01-slave-1.2.17.bin), but file names can change — copy it, rename it, download it from somewhere else, and the name starts lying. CRC32 does not: it identifies these exact bytes. diff --git a/python/tests/conftest.py b/python/tests/conftest.py index cdb1d7d..bbf84a4 100644 --- a/python/tests/conftest.py +++ b/python/tests/conftest.py @@ -19,6 +19,9 @@ from __future__ import annotations +import importlib.machinery +import importlib.util +import os import pathlib import sys @@ -30,6 +33,30 @@ if _REPO_PYTHON not in sys.path: sys.path.insert(0, _REPO_PYTHON) +# Explicit isolated native build for validation without replacing the checkout's +# live extension. Default remains strict in-tree loading. Never just relabel +# __file__: load the actual module and verify its resolved location below. +_native_dir_value = os.environ.get("TACCAP_TEST_NATIVE_DIR") +_expected_native_dir = None +if _native_dir_value: + _expected_native_dir = pathlib.Path(_native_dir_value).resolve(strict=True) + if not _expected_native_dir.is_dir(): + raise RuntimeError("TACCAP_TEST_NATIVE_DIR must name a directory") + _candidates = [ + _expected_native_dir / ("_taccap_native" + suffix) + for suffix in importlib.machinery.EXTENSION_SUFFIXES + if (_expected_native_dir / ("_taccap_native" + suffix)).is_file() + ] + if len(_candidates) != 1: + raise RuntimeError( + "TACCAP_TEST_NATIVE_DIR must contain exactly one native extension" + ) + _name = "xense.taccap._taccap_native" + _spec = importlib.util.spec_from_file_location(_name, _candidates[0]) + _module = importlib.util.module_from_spec(_spec) + _spec.loader.exec_module(_module) + sys.modules[_name] = _module + import xense.taccap as _t # noqa: E402 (must follow the path surgery above) assert _REPO_PYTHON in _t.__file__, ( @@ -41,7 +68,11 @@ # checkout and site-packages, so __file__ pointing here does NOT prove the # compiled extension came from here. Check the extension separately. _native = sys.modules.get("xense.taccap._taccap_native") -assert _native is not None and _REPO_PYTHON in _native.__file__, ( +assert _native is not None and ( + pathlib.Path(_native.__file__).resolve().parent == _expected_native_dir + if _expected_native_dir + else _REPO_PYTHON in _native.__file__ +), ( f"_taccap_native came from {getattr(_native, '__file__', None)}, not this " "checkout — rebuild with `uv pip install -e . --no-build-isolation`." ) diff --git a/python/tests/test_dispatch_decoupling.py b/python/tests/test_dispatch_decoupling.py index 2f831e5..6552feb 100644 --- a/python/tests/test_dispatch_decoupling.py +++ b/python/tests/test_dispatch_decoupling.py @@ -63,6 +63,13 @@ def _encoder_frame(seq: int) -> bytes: "queue_dropped", "queue_high_water", "callback_max_us", + "control_queue_dropped", + "control_queue_high_water", + "control_queue_overflows", + "parser_no_head", + "parser_oversize", + "parser_short_body", + "parser_bad_length", ) @@ -95,6 +102,21 @@ def test_stop_with_a_python_subscriber_does_not_crash(fake_mcu): transport.stop() # must not segfault; fixture stops again +def test_receive_trace_is_opt_in_and_contains_raw_bytes(fake_mcu): + transport, master = fake_mcu + assert transport.take_receive_trace()["records"] == [] + transport.enable_receive_trace() + wire = _encoder_frame(19) + os.write(master, wire) + assert _wait_until(lambda: transport.stats.frames_received >= 1) + trace = transport.take_receive_trace() + assert trace["overwritten_records"] == 0 + inputs = [r for r in trace["records"] if r["kind"] == "input"] + assert b"".join(r["bytes"] for r in inputs) == wire + assert all(r["host_time_ns"] > 0 and r["reason"] == 0 for r in inputs) + transport.enable_receive_trace(False) + + def test_slow_python_callback_does_not_stall_the_reader(fake_mcu): """The regression that caused the customer-visible rate drop. diff --git a/python/tests/test_follower_imu_surface.py b/python/tests/test_follower_imu_surface.py new file mode 100644 index 0000000..4a58173 --- /dev/null +++ b/python/tests/test_follower_imu_surface.py @@ -0,0 +1,23 @@ +"""The Python opt-in stays optional; the leader's existing signature is intact.""" + +import re + +import xense.taccap as t + + +def test_follower_keeps_motor_default_and_opts_in_to_imu(): + doc = t.FollowerGripper.start_streaming.__doc__ + assert re.search(r"motor_hz: [^,]* = 100", doc) + assert re.search(r"imu_hz: [^,)]* = 0", doc) + + +def test_leader_signature_is_unchanged(): + doc = t.LeaderGripper.start_streaming.__doc__ + assert re.search(r"imu_hz: [^,]* = 100", doc) + assert re.search(r"encoder_hz: [^,)]* = 100", doc) + + +def test_calibration_has_explicit_long_timeout(): + doc = t.IMU.calibrate_accel_gyro.__doc__ + assert "16000" in doc + assert "disable" in doc diff --git a/python/tests/test_imu_time.py b/python/tests/test_imu_time.py new file mode 100644 index 0000000..2a0b36f --- /dev/null +++ b/python/tests/test_imu_time.py @@ -0,0 +1,84 @@ +"""Native V2 timestamp width and boot identity survive the Python boundary.""" + +import os +import pty +import select +import struct +import threading + +import xense.taccap as t + + +def test_native_64bit_time_and_boot_session_over_pty(): + master, slave = pty.openpty() + stopped = threading.Event() + errors = [] + capture = (5 << 32) + 1234 + + def device(): + parser = t.FrameParser() + try: + while not stopped.is_set(): + if not select.select([master], [], [], 0.02)[0]: + continue + parser.feed(os.read(master, 4096)) + while (frame := parser.pop()) is not None: + payload = b"\x00" + if frame.cmd == t.Cmd.GetVersion: + payload = bytes([1, 2, 6, 0]) + elif frame.cmd == t.Cmd.GetSn: + payload = b"TCGU01A25Z0016m\0" + elif frame.cmd == t.Cmd.TimeSession: + assert len(frame.payload) == 8 + payload = struct.pack("