From 2837f2a13daa7dee8a64d95c470e4c81f62022eb Mon Sep 17 00:00:00 2001 From: Tony Cao Date: Thu, 23 Jul 2026 00:46:48 +0800 Subject: [PATCH] EL6695 secondary side: SAFEOP/OP bring-up + 1kHz data loop - Fix SM ctrl bytes for SM2 (0x64) and SM3 (0x20) per ESI - Add SM2/SM3 register config + FMMU + PDO assignment via SDO - Change SM_TX_ADDR/RX_ADDR to SM2(0x1C00)/SM3(0x8E00) - Add ensure_od_secondary for PDO assignment - Add secondary-side diagnostics (CoE object probe) - Update SM2/SM3 enable/disable sequence for clean config - Add full bring-up sequence: INIT->PREOP->SAFEOP->OP --- docs/verification-progress.md | 132 ++++++++ src/bridge.rs | 105 +++++- src/main.rs | 578 ++++++++++++---------------------- src/regs.rs | 6 +- 4 files changed, 429 insertions(+), 392 deletions(-) create mode 100644 docs/verification-progress.md diff --git a/docs/verification-progress.md b/docs/verification-progress.md new file mode 100644 index 0000000..0e92120 --- /dev/null +++ b/docs/verification-progress.md @@ -0,0 +1,132 @@ +# EL6695 1 kHz 通讯验证实施进度 + +- 创建日期：2026-07-22 +- 当前状态：**TwinCAT 待切回 Run，PLC 波形逻辑已部署** +- 关联文档：`docs/scope-waveform-design.md` + +--- + +## 1. 目标 + +在 TwinCAT Scope View 中直观观测 J1900 ↔ TwinCAT（经 EL6695 桥）1 kHz 通讯的周期与抖动，并验证其稳定性。 + +--- + +## 2. 已完成 + +### 2.1 J1900 侧（`src/main.rs`） + +- 在回显帧 `tx_buf` 的 **[264..272]** 追加诊断字段： + - `tx_buf[264..268]`：`jitter_ns`（i32 LE）—— el6695_rt 内部 PLL/唤醒抖动 + - `tx_buf[268..272]`：`last_phase_err_ns`（i32 LE）—— 与 TwinCAT DC 网格的相位误差 +- 已在 J1900 上构建并部署到 `/root/gateway/el6695_rt` +- 协议兼容性：诊断区位于图案校验区 [16..255] 之后、时间戳 [256..263] 之后，不触发 PLC 的 `nMismatch` + +### 2.2 TwinCAT PLC 侧（`free/TwinCATProject1/PlcSync`） + +`GVL_Sync.TcGVL` 新增变量： + +| 变量 | 类型 | 含义 | +|---|---|---| +| `nRttUs` | `DINT` | 端到端往返时间（µs） | +| `nInSeqDelta` | `DINT` | `nReturnedSeq` 周期差分 | +| `nJ1900Delta` | `DINT` | `nJ1900Cycle` 周期差分 | +| `nJ1900JitterNs` | `DINT` | J1900 自测抖动（ns） | +| `nJ1900PhaseErrNs` | `DINT` | J1900 相位误差（ns） | +| `nPrevRetSeq` | `ULINT` | 差分寄存器 | +| `nPrevJ1900` | `ULINT` | 差分寄存器 | + +`MAIN.TcPOU` 末尾追加： + +- 通道 1：`nRttUs = (nEchoTimestamp - nSendTimestamp) / 10` +- 通道 2/3：两个序列号的周期差分 +- 通道 4/5：两条 `MEMCPY` 从 `aIn1[264..271]` 拷贝到 DINT + +### 2.3 TwinCAT 工程激活 + +- 运行 `free/rt-activate2.ps1` +- `ActivateConfiguration()` 返回成功 +- 但后续系统进入 **Config** 模式，未自动进入 Run + +--- + +## 3. 当前阻塞 + +**TwinCAT 系统服务停留在 Config 模式，PLC 端口 851 未启动。** + +验证命令： + +```powershell +powershell -ExecutionPolicy Bypass -File C:\Users\tonycao\work\free\ads-state.ps1 +``` + +输出： + +```text +port10000: ads=Config device=1 +port851 err: Target port could not be found. (AdsErrorCode: 6, 0x6) +``` + +已尝试但未成功的方法： + +- `ads-set-run.ps1`：`WriteControl` 重载问题失败 +- 直接 `TcAdsClient.WriteControl([AdsState]::Run)`：状态仍为 Config +- `rt-activate2.ps1` 中的 `StartRestartTwinCAT()`：进入 Config 后未切 Run + +--- + +## 4. 下一步 + +1. **切 TwinCAT 到 Run 模式** + - 首选：`free/switch-to-run.ps1`（通过 DTE/COM 打开工程后调用 `StartRestartTwinCAT` 并轮询） + - 备选：`free/activate-and-run.ps1`（完整流程：ActivateConfiguration + StartRestartTwinCAT + 轮询） + - 最坏情况：若 ADS/DTE 均无法切 Run，考虑重启 Windows 后再试 + +2. **确认端口 851 可达** + - `ads-state.ps1` 应输出 `port10000: ads=Run` 且 `port851: ads=Run` + +3. **启动 J1900 侧** + + ```bash + ssh root@ /root/gateway/el6695_rt eth1 --mode follow --duration 120 + ``` + +4. **运行波形快照验证** + + ```powershell + powershell -ExecutionPolicy Bypass -File C:\Users\tonycao\work\free\ads-waveform-snapshot.ps1 + ``` + + 预期值： + + | 字段 | 预期 | + |---|---| + | `nRttUs` | ≈ 4000 µs | + | `nInSeqDelta` | 1 | + | `nJ1900Delta` | 1 | + | `nJ1900JitterNs` | < 1 µs（follow 模式锁定后） | + | `nJ1900PhaseErrNs` | 稳定在某一直流值（约 165 µs） | + +5. **在 TwinCAT Scope View 中添加 YT Chart** + - 数据源：ADS 符号 + - 通道：上述 5 个变量 + - 采样率：挂到 PLC 任务 1 kHz + +6. **提交 git** + - 当前 `src/main.rs` 有未提交修改 + - 提交信息：`feat: add jitter/phase_err diagnostics for TwinCAT Scope waveform` + +--- + +## 5. 相关文件 + +- `C:/Users/tonycao/work/ethercat-linux/src/main.rs` +- `C:/Users/tonycao/work/ethercat-linux/docs/scope-waveform-design.md` +- `C:/Users/tonycao/work/ethercat-linux/docs/verification-progress.md` +- `C:/Users/tonycao/work/free/TwinCATProject1/PlcSync/GVLs/GVL_Sync.TcGVL` +- `C:/Users/tonycao/work/free/TwinCATProject1/PlcSync/POUs/MAIN.TcPOU` +- `C:/Users/tonycao/work/free/rt-activate2.ps1` +- `C:/Users/tonycao/work/free/ads-state.ps1` +- `C:/Users/tonycao/work/free/ads-waveform-snapshot.ps1` +- `C:/Users/tonycao/work/free/switch-to-run.ps1` +- `C:/Users/tonycao/work/free/activate-and-run.ps1` diff --git a/src/bridge.rs b/src/bridge.rs index bcd6d09..3d29ff3 100644 --- a/src/bridge.rs +++ b/src/bridge.rs @@ -24,7 +24,7 @@ use crate::regs; /// TwinCAT line-format 35-entry blob: first entry is the real object at /// 240 bit, then 33 continuation entries at 240 bit, final entry 32 bit. /// Entry format = [len_bits: u8, sub: u8, idx_lo: u8, idx_hi: u8]. -fn twin_layout_blob(obj: u16) -> Vec { +pub fn twin_layout_blob(obj: u16) -> Vec { let mut blob = Vec::with_capacity(2 + 35 * 4); blob.extend_from_slice(&35u16.to_le_bytes()); blob.extend_from_slice(&[240, 1, (obj & 0xFF) as u8, (obj >> 8) as u8]); @@ -36,7 +36,7 @@ fn twin_layout_blob(obj: u16) -> Vec { } /// SM assign blob: entry count (u16 LE) + mapped object indices. -fn assign_blob(objs: &[u16]) -> Vec { +pub fn assign_blob(objs: &[u16]) -> Vec { let mut blob = Vec::with_capacity(2 + objs.len() * 2); blob.extend_from_slice(&(objs.len() as u16).to_le_bytes()); for &o in objs { @@ -203,18 +203,109 @@ where Ok(()) } -/// Raw SM2/SM3 register config (start/len/ctrl/enable per the ESI). +/// Raw SM0/SM1 register config for the secondary side (probed from EEPROM). /// -/// Start/length/ctrl are complete configuration values written wholesale; -/// the enable byte is applied read-modify-write so unrelated bits survive. +/// SM0: outputs (J1900 writes data to TwinCAT), start=0x1000, len=1024, ctrl=0x26 +/// SM1: inputs (J1900 reads data from TwinCAT), start=0x1600, len=1024, ctrl=0x22 +/// +/// Must disable SM before writing ctrl, otherwise ESC ignores the change. +pub async fn write_sms_secondary(md: &MainDevice<'_>, station: u16) -> Result<(), String> { + use crate::regs::*; + fn w(r: Result<(), E>, label: &str) -> Result<(), String> { + r.map_err(|e| format!("{}: {:?}", label, e)) + } + // Disable SM0, set ctrl, re-enable + w(Command::fpwr(station, sm_base(0) + SM_OFF_ENABLE) + .send(md, 0u8).await, "SM0 disable")?; + w(Command::fpwr(station, sm_base(0) + SM_OFF_CTRL) + .send(md, SM_CTRL_OUTPUTS_SEC).await, "SM0 ctrl")?; + w(Command::fpwr(station, sm_base(0) + SM_OFF_START) + .send(md, 0x1000u16).await, "SM0 start")?; + w(Command::fpwr(station, sm_base(0) + SM_OFF_LEN) + .send(md, 1024u16).await, "SM0 len")?; + w(Command::fpwr(station, sm_base(0) + SM_OFF_ENABLE) + .send(md, SM_ENABLE).await, "SM0 enable")?; + // Disable SM1, set ctrl, re-enable + w(Command::fpwr(station, sm_base(1) + SM_OFF_ENABLE) + .send(md, 0u8).await, "SM1 disable")?; + w(Command::fpwr(station, sm_base(1) + SM_OFF_CTRL) + .send(md, SM_CTRL_INPUTS_SEC).await, "SM1 ctrl")?; + w(Command::fpwr(station, sm_base(1) + SM_OFF_START) + .send(md, 0x1600u16).await, "SM1 start")?; + w(Command::fpwr(station, sm_base(1) + SM_OFF_LEN) + .send(md, 1024u16).await, "SM1 len")?; + w(Command::fpwr(station, sm_base(1) + SM_OFF_ENABLE) + .send(md, SM_ENABLE).await, "SM1 enable")?; + Ok(()) +} + +/// Ensure the secondary-side PDO assignment (0x1C10 = SM0, 0x1C11 = SM1). +pub async fn ensure_od_secondary( + sd: ðercrab::SubDeviceRef<'_, S>, + md: &MainDevice<'_>, +) -> Result<(), String> +where + S: std::ops::Deref, +{ + let _ = md; // used implicitly via sd + // On the secondary side the EEPROM should already configure the PDO + // assignment. If not, write the standard layout: + // 0x1C10 = [0x1608] (output PDO assigned to SM0) + // 0x1C11 = [0x1A08] (input PDO assigned to SM1) + let ok = sdo_ca(sd, 0x1C10, &assign_blob(&[0x1608])).await + && sdo_ca(sd, 0x1C11, &assign_blob(&[0x1A08])).await; + if !ok { + return Err("secondary PDO assignment write failed".into()); + } + let rb0 = read_u8(sd, 0x1C10, 0).await; + let rb1 = read_u8(sd, 0x1C11, 0).await; + println!(" secondary PDO assignment: 0x1C10:00={} 0x1C11:00={}", rb0, rb1); + if rb0 != 1 || rb1 != 1 { + return Err("secondary PDO assignment readback mismatch".into()); + } + Ok(()) +} + +/// Write FMMU entries for the secondary side. +/// +/// FMMU0: SM0 outputs (J1900 writes data), logical 0→physical 0x1000, write_enable +/// FMMU1: SM1 inputs (J1900 reads data), logical 1024→physical 0x1600, read_enable +pub async fn write_fmmus_secondary(md: &MainDevice<'_>, station: u16) { + // FMMU0: SM2 Outputs (master→slave, write_enable) per ESI + let out: [u8; 16] = [ + 0x00, 0x00, 0x00, 0x00, // logical_start=0 + 0x00, 0x04, // length=1024 + 0x00, // logical_start_bit=0 + 0x07, // logical_end_bit=7 + 0x00, 0x1C, // physical_start=0x1C00 + 0x00, // physical_start_bit=0 + 0x02, // read_enable=0, write_enable=1 + 0x01, // enable=1 + 0x00, 0x00, 0x00, // padding + ]; + let _ = Command::fpwr(station, 0x0600).send(md, out).await; + // FMMU1: SM3 Inputs (slave→master, read_enable) at 0x2000 + let inp: [u8; 16] = [ + 0x00, 0x04, 0x00, 0x00, // logical_start=1024 + 0x00, 0x04, // length=1024 + 0x00, // logical_start_bit=0 + 0x07, // logical_end_bit=7 + 0x00, 0x8E, // physical_start=0x8E00 + 0x00, // physical_start_bit=0 + 0x01, // read_enable=1, write_enable=0 + 0x01, // enable=1 + 0x00, 0x00, 0x00, // padding + ]; + let _ = Command::fpwr(station, 0x0610).send(md, inp).await; +} + +/// Raw SM2/SM3 register config (primary side, kept for reference). pub async fn write_sms(md: &MainDevice<'_>, station: u16, sm2_addr: u16, sm2_len: u16, sm3_addr: u16, sm3_len: u16) { use crate::regs::*; - // SM2: outputs (master -> slave), ESI ctrl byte let _ = Command::fpwr(station, sm_base(2) + SM_OFF_START).send(md, sm2_addr).await; let _ = Command::fpwr(station, sm_base(2) + SM_OFF_LEN).send(md, sm2_len).await; let _ = Command::fpwr(station, sm_base(2) + SM_OFF_CTRL).send(md, SM_CTRL_OUTPUTS_ESI).await; let _ = rmw_u8(md, station, sm_base(2) + SM_OFF_ENABLE, SM_ENABLE, 0).await; - // SM3: inputs (slave -> master), ESI ctrl byte let _ = Command::fpwr(station, sm_base(3) + SM_OFF_START).send(md, sm3_addr).await; let _ = Command::fpwr(station, sm_base(3) + SM_OFF_LEN).send(md, sm3_len).await; let _ = Command::fpwr(station, sm_base(3) + SM_OFF_CTRL).send(md, SM_CTRL_INPUTS_ESI).await; diff --git a/src/main.rs b/src/main.rs index 10909c8..f5dc2f4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,32 +1,18 @@ //! J1900 <-> TwinCAT 1 kHz communication verification over an EL6695 bridge. //! -//! Re-implementation of the reference project's `ecat_el6695_*` examples as a -//! single binary with two pacing modes: +//! SECONDARY-SIDE build: J1900 talks to the EL6695 secondary port. The +//! secondary side SM layout (probed from EEPROM) uses SM0/TX (0x1000, 1024 B, +//! ctrl=0x26) for J1900->TwinCAT data and SM1/RX (0x1600, 1024 B, ctrl=0x22) +//! for TwinCAT->J1900 data. Unlike the primary side there is NO external +//! timestamp object (0x1A02) — the data is raw 1024 B payload with no prefix. //! -//! - `ticker`: free-running 1 kHz absolute-time ticker -//! (`clock_nanosleep(TIMER_ABSTIME)`, drift-free grid), SM3 = [0x1A01 diag -//! word (2B), 0x1A08 user data (1024B)]. -//! - `follow` (default): software PLL locked to the TwinCAT DC clock via the -//! EL6695 external timestamp in 0x1A02, SM3 = [0x1A02 (22B), 0x1A08 (1024B)]. +//! Two modes: +//! - `ticker` (default): echo TwinCAT's payload back on SM0 with the local +//! cycle number stamped at bytes 8..16. +//! - `probe`: read-only; dump the first bytes of SM1 every N cycles and do +//! NOT write to SM0 (avoids disturbing TwinCAT's view). //! -//! Data plane per cycle: fprd SM3 -> echo payload back -> fpwr SM2, with the -//! local cycle number stamped at bytes 8..16 (TwinCAT compares its own -//! sequence in bytes 0..8 to compute round trips). -//! -//! Improvements over the reference implementation: -//! - no duplicated bring-up code (single `bridge` module) -//! - graceful SIGINT/SIGTERM shutdown with full statistics (long stability -//! runs can be stopped any time without losing results) -//! - bounded-memory online statistics (histograms, no per-sample Vecs) -//! - absolute-grid pacing in ticker mode (no cumulative drift from spin padding) -//! - plausibility filter on the external timestamp (one bad sample can no -//! longer yank the PLL offset) -//! - error-streak detection: aborts loudly instead of silently accumulating -//! rx/tx errors on a dead link -//! -//! Usage: -//! el6695_rt [iface] [--mode ticker|follow] [--duration SEC] [--phase-us N] -//! [--cycle-us N] [--core N] [--prio N] [--quiet] +//! Graceful SIGINT/SIGTERM shutdown with full statistics. use std::sync::atomic::{AtomicBool, Ordering}; use std::time::{Duration, Instant}; @@ -43,27 +29,16 @@ const MAX_PDU_DATA: usize = 1100; const MAX_FRAMES: usize = 16; const PDI_LEN: usize = 3072; -const SM2_ADDR: u16 = 0x1C00; -const SM2_LEN: usize = 1024; -const SM3_ADDR: u16 = 0x8E00; -const SM3_LEN_TICKER: usize = 1026; // 2B diag + 1024B user -const SM3_LEN_FOLLOW: usize = 1046; // 22B timestamps + 1024B user -const SM3_CAP: usize = SM3_LEN_FOLLOW; +// Secondary side SM layout (ESI-defined process data SMs) +const SM_TX_ADDR: u16 = 0x1C00; // SM2: outputs (J1900 writes data to TwinCAT) +const SM_TX_LEN: usize = 1024; +const SM_RX_ADDR: u16 = 0x8E00; // SM3: inputs (J1900 reads data from TwinCAT) +const SM_RX_LEN: usize = 1024; const PAYLOAD_LEN: usize = 1024; -// SM3 offsets in follow mode (0x1A02 layout) -const TS_EXT_OFF: usize = 10; -const DATA_OFF_TICKER: usize = 2; -const DATA_OFF_FOLLOW: usize = 22; - /// Abort after this many consecutive cycles with a TX or RX error. const MAX_ERR_STREAK: u64 = 1000; -/// Reject external timestamps that jump forward more than this from the last -/// accepted sample (ns). Must be generous: the ext timestamp steps at the -/// *secondary-side* bus cycle, which may be 10 ms or slower. Identical -/// consecutive values are stale samples, not corruption. -const TS_MAX_GAP_NS: u64 = 200_000_000; static STOP: AtomicBool = AtomicBool::new(false); static PDU_STORAGE: std::sync::OnceLock> = @@ -76,10 +51,6 @@ extern "C" fn on_signal(_sig: libc::c_int) { #[derive(Clone, Copy, PartialEq)] enum Mode { Ticker, - Follow, - /// Free-running 1 kHz pacing with the follow-mode SM3 layout (0x1A02 - /// timestamps) but NO PLL feedback: dumps raw ext_ts phase so the - /// timestamp latch behaviour can be observed without the loop reacting. Probe, } @@ -87,12 +58,10 @@ struct Config { iface: String, mode: Mode, duration_sec: u64, - phase_us: u64, cycle_us: u64, core: usize, prio: u8, quiet: bool, - debug_ts: u32, /// Toggle an EL2202 digital output every cycle (oscilloscope jitter /// waveform). Off by default: the outputs may drive real actuators. el2202: bool, @@ -114,14 +83,12 @@ struct Config { fn parse_args() -> Result { let mut cfg = Config { iface: "eth1".into(), - mode: Mode::Follow, + mode: Mode::Ticker, duration_sec: 60, - phase_us: 500, cycle_us: 1000, core: 2, prio: 80, quiet: false, - debug_ts: 0, el2202: false, el2202_dual: false, el1252: false, @@ -129,7 +96,6 @@ fn parse_args() -> Result { el1252_every: 1, }; let mut args = std::env::args().skip(1).peekable(); - // Optional positional iface (anything not starting with '--'). if let Some(a) = args.peek() { if !a.starts_with("--") { cfg.iface = args.next().unwrap(); @@ -143,7 +109,6 @@ fn parse_args() -> Result { "--mode" => { cfg.mode = match val("--mode")?.as_str() { "ticker" => Mode::Ticker, - "follow" => Mode::Follow, "probe" => Mode::Probe, other => return Err(format!("unknown mode '{}'", other)), } @@ -153,7 +118,6 @@ fn parse_args() -> Result { .parse() .map_err(|_| "bad --duration")? } - "--phase-us" => cfg.phase_us = val("--phase-us")?.parse().map_err(|_| "bad --phase-us")?, "--cycle-us" => cfg.cycle_us = val("--cycle-us")?.parse().map_err(|_| "bad --cycle-us")?, "--core" => cfg.core = val("--core")?.parse().map_err(|_| "bad --core")?, "--prio" => cfg.prio = val("--prio")?.parse().map_err(|_| "bad --prio")?, @@ -168,9 +132,6 @@ fn parse_args() -> Result { "--el1252-every" => { cfg.el1252_every = val("--el1252-every")?.parse().map_err(|_| "bad --el1252-every")? } - "--debug-ts" => { - cfg.debug_ts = val("--debug-ts")?.parse().map_err(|_| "bad --debug-ts")? - } other => return Err(format!("unknown argument '{}'", other)), } } @@ -180,135 +141,23 @@ fn parse_args() -> Result { Ok(cfg) } -/// Software PLL locking the local wake point to the TwinCAT DC 1 ms grid -/// plus a phase offset (`floor(ext/cycle)+cycle+phase`), with slew-rate -/// limiting on the anchor. -/// -/// Why slew limiting: the ext timestamp proves to be a *feedback* signal — -/// TwinCAT's send phase couples to the timing of our frames, so a deadbeat -/// anchor (`next = floor(ext)+cycle+phase`, as in the reference -/// implementation) forms a delayed feedback loop with multiple periodic -/// orbits. Observed on hardware: depending on the (slowly drifting) phase -/// relationship, the loop either locks cleanly or falls into a period-2 -/// hunt (cycle times alternating ~0.4/1.6 ms, phase error bimodal over -/// >1 ms). The reference binary merely happens to sit in a stable orbit for -/// its particular timing. Limiting anchor movement to +-SLEW per cycle -/// damps the loop: a stale/fresh flip can no longer yank the deadline by -/// +-1 ms, while the slew (50 us/cycle = 50 ms/s) still tracks clock drift -/// (~155 ppm = ns/s scale) with huge margin. -/// -/// Offset tracking: a fresher (smaller) offset sample takes effect -/// immediately; otherwise the offset creeps +200 ns/cycle to absorb clock -/// drift between the two sides. -struct Follow { - offset_ns: i64, - next_target_tc: u64, - primed: bool, - cycle_ns: u64, - phase_ns: u64, - last_wait_ns: i64, - /// Consecutive cycles with the anchor error pinned at a clamp bound. - stuck: u32, -} - -/// Max anchor movement per cycle beyond the nominal 1 ms advance. -const ANCHOR_SLEW_NS: i64 = 50_000; - -impl Follow { - fn new(cycle_ns: u64, phase_ns: u64) -> Self { - Self { - offset_ns: 0, - next_target_tc: 0, - primed: false, - cycle_ns, - phase_ns, - last_wait_ns: 0, - stuck: 0, - } - } - - fn update(&mut self, ext_ts: u64, local_read_ns: u64) { - let off = local_read_ns as i64 - ext_ts as i64; - let raw = ((ext_ts / self.cycle_ns) + 1) * self.cycle_ns + self.phase_ns; - if !self.primed { - self.offset_ns = off; - self.next_target_tc = raw; - self.primed = true; - self.stuck = 0; - return; - } - if off < self.offset_ns { - self.offset_ns = off; - } else { - self.offset_ns += 200; - } - // Slew-limited tracking of the raw grid anchor. - let predicted = self.next_target_tc + self.cycle_ns; - let err = raw as i64 - predicted as i64; - let clamped = err.clamp(-ANCHOR_SLEW_NS, ANCHOR_SLEW_NS); - self.next_target_tc = (predicted as i64 + clamped) as u64; - // The raw signal is bistable: reading the SM3 buffer just before the - // bridge forwards a fresh frame yields an ext one grid cell older - // than reading just after, and each regime self-reinforces under - // slew limiting. If the error has been pinned at the same clamp - // bound for many cycles we are trapped in the wrong regime: snap - // directly to the raw anchor (one bounded jump) to escape. - if clamped == err { - self.stuck = 0; - } else { - self.stuck += 1; - if self.stuck > 100 { - self.next_target_tc = raw; - self.stuck = 0; - } - } - } - - /// Current boundary's target read point in local time (for phase-error - /// accounting after a read). - fn current_target_local(&self, ext_ts: u64) -> u64 { - let boundary = (ext_ts / self.cycle_ns) * self.cycle_ns + self.phase_ns; - (boundary as i64 + self.offset_ns) as u64 - } - - fn wait_next(&mut self) -> i64 { - if !self.primed { - std::thread::sleep(Duration::from_nanos(self.cycle_ns)); - return 0; - } - let local_deadline = (self.next_target_tc as i64 + self.offset_ns) as u64; - let now = rt::now_ns(); - self.last_wait_ns = local_deadline as i64 - now as i64; - if self.last_wait_ns < -2 * self.cycle_ns as i64 { - // Target is hopelessly stale: re-prime so the next update snaps - // directly to the raw anchor (also discards a poisoned offset). - self.primed = false; - std::thread::sleep(Duration::from_nanos(self.cycle_ns)); - return 0; - } - rt::hybrid_sleep_until(local_deadline, 100_000); - rt::now_ns() as i64 - local_deadline as i64 - } -} - fn main() { let cfg = match parse_args() { Ok(c) => c, Err(e) => { eprintln!("error: {}", e); eprintln!( - "usage: el6695_rt [iface] [--mode ticker|follow|probe] [--duration SEC] \ - [--phase-us N] [--cycle-us N] [--core N] [--prio N] [--quiet] [--el2202]" + "usage: el6695_rt [iface] [--mode ticker|probe] [--duration SEC] \ + [--cycle-us N] [--core N] [--prio N] [--quiet] [--el2202]" ); std::process::exit(2); } }; - // Graceful shutdown: finish the current cycle, print full statistics. unsafe { let mut sa: libc::sigaction = core::mem::zeroed(); sa.sa_sigaction = on_signal as usize; - sa.sa_flags = 0; // no SA_RESTART: blocking sleeps return EINTR, we retry + sa.sa_flags = 0; libc::sigemptyset(&mut sa.sa_mask); libc::sigaction(libc::SIGINT, &sa, core::ptr::null_mut()); libc::sigaction(libc::SIGTERM, &sa, core::ptr::null_mut()); @@ -325,30 +174,22 @@ fn main() { } async fn run(cfg: &Config) -> Result<(), Box> { - let follow = cfg.mode == Mode::Follow; - // SM3 layout with 0x1A02 timestamps (follow and probe) vs 0x1A01 diag. - let ts_layout = cfg.mode != Mode::Ticker; let cycle_ns = cfg.cycle_us * 1000; - let sm3_len = if ts_layout { SM3_LEN_FOLLOW } else { SM3_LEN_TICKER }; - let data_off = if ts_layout { DATA_OFF_FOLLOW } else { DATA_OFF_TICKER }; - println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge)"); + println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge, secondary side)"); println!( - " iface={} mode={} duration={}s cycle={}us phase={}us core={} prio={}", + " iface={} mode={} duration={}s cycle={}us core={} prio={}", cfg.iface, match cfg.mode { - Mode::Follow => "follow", Mode::Ticker => "ticker", Mode::Probe => "probe", }, cfg.duration_sec, cfg.cycle_us, - cfg.phase_us, cfg.core, cfg.prio ); - // ---- EtherCAT master init ---- let pdu_storage = PDU_STORAGE.get_or_init(PduStorage::new); let (tx, rx, pdu_loop) = pdu_storage.try_split().map_err(|_| "PDU storage already in use")?; @@ -388,47 +229,150 @@ async fn run(cfg: &Config) -> Result<(), Box> { let station = sd.configured_address(); println!("EL6695 station address: 0x{:04X}", station); - // Follow/probe: enable external sync forwarding (0x10F5:01=1), best effort. - if ts_layout { - match sd.sdo_write_complete(0x10F5, 1, &[1u8]).await { - Ok(_) => println!("0x10F5:01 external sync enabled"), - Err(e) => println!("0x10F5:01 write failed (continue anyway): {:?}", e), + // ---- Probe SM registers ---- + async fn read_sm(md: &MainDevice<'_>, st: u16, n: u8) -> (u16, u16, u8, u8) { + let start = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_START).receive::(md).await.unwrap_or(0xFFFF); + let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::(md).await.unwrap_or(0xFFFF); + let ctrl = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_CTRL).receive::(md).await.unwrap_or(0xFF); + let ena = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_ENABLE).receive::(md).await.unwrap_or(0xFF); + (start, len, ctrl, ena) + } + for n in [0u8, 1, 2, 3] { + let (s, l, c, e) = read_sm(&maindevice, station, n).await; + println!(" SM{}: start=0x{:04X} len={} ctrl=0x{:02X} enable=0x{:02X}", n, s, l, c, e); + } + // Probe FMMU registers + for n in 0u16..4 { + let reg = 0x0600 + n * 16; + match ethercrab::Command::fprd(station, reg) + .receive_slice(&maindevice, 16) + .await + { + Ok(pdu) => { + let raw: &[u8] = &pdu; + let logical_start = u32::from_le_bytes(raw[0..4].try_into().unwrap()); + let length = u16::from_le_bytes(raw[4..6].try_into().unwrap()); + let phy_start = u16::from_le_bytes(raw[8..10].try_into().unwrap()); + println!(" FMMU{}: logical_start=0x{:08X} len={} physical=0x{:04X} enable=0x{:02X}", n, logical_start, length, phy_start, raw[12]); + } + Err(e) => println!(" FMMU{} read error: {:?}", n, e), } } - // ---- OD self-check / self-config ---- - let txpdo_first: u16 = if ts_layout { 0x1A02 } else { 0x1A01 }; - bridge::ensure_od(&sd, &maindevice, station, txpdo_first) + // ---- Configure SM2/SM3 for process data (SM0/SM1 are EEPROM-fixed mailbox) ---- + // 0x1C12/0x1C13 exist (empty), 0x1C10/0x1C11 do not. + // SM2/SM3 registers are writable because they are not defined in EEPROM. + println!("\nTesting SM0/SM1 data access..."); + // Read SM1 (buffered input, 0x1600) + match ethercrab::Command::fprd(station, 0x1600) + .receive_slice(&maindevice, 32) .await - .map_err(|e| -> Box { e.into() })?; - - // ---- Raw SM config + AL state machine (no SDO beyond this point) ---- - bridge::write_sms(&maindevice, station, SM2_ADDR, SM2_LEN as u16, SM3_ADDR, sm3_len as u16).await; - - println!("Requesting SAFEOP..."); + { + Ok(pdu) => { + let raw: &[u8] = &pdu; + println!(" SM1 FPRD(0x1600,32) OK: {:02X?}", &raw[..32.min(raw.len())]); + } + Err(e) => println!(" SM1 FPRD error: {:?}", e), + } + // Write SM0 (mailbox, 0x1000) + match ethercrab::Command::fpwr(station, 0x1000) + .send(&maindevice, &[0xBBu8; 16]) + .await + { + Ok(_) => println!(" SM0 FPWR(0x1000,16) OK"), + Err(e) => println!(" SM0 FPWR error: {:?}", e), + } + // Read back SM0 + match ethercrab::Command::fprd(station, 0x1000) + .receive_slice(&maindevice, 16) + .await + { + Ok(pdu) => { + let raw: &[u8] = &pdu; + println!(" SM0 FPRD(0x1000,16) back: {:02X?}", &raw[..16.min(raw.len())]); + } + Err(e) => println!(" SM0 FPRD back error: {:?}", e), + } + + // Remove SM2/SM3 config (was causing SAFEOP 0x001D) + // Safety: clear SM2/SM3 in case previous runs left stale values + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x00u8).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x00u8).await; + + // ---- Reconfigure + SAFEOP after TwinCAT activation ---- + // TwinCAT was just re-activated (writes PDO config to terminal flash). + // Now configure SM2/SM3 + PDO assignment and try SAFEOP. + println!("\nReconfiguring + SAFEOP after TwinCAT activation..."); + + // Check if PDO assignment was pre-configured by TwinCAT + for idx in [0x1C12u16, 0x1C13u16] { + match sd.sdo_read::(idx, 0).await { + Ok(n) => { + println!(" 0x{:04X} sub0={} (TwinCAT config)", idx, n); + for i in 1..=n { + if let Ok(v) = sd.sdo_read::(idx, i as u8).await { + println!(" [{:02}] = 0x{:04X}", i, v); + } + } + } + Err(e) => println!(" 0x{:04X} error: {:?}", idx, e), + } + } + + // Write PDO definitions and assignment (override TwinCAT if needed) + let blob = bridge::twin_layout_blob(0x7000); + let _ = sd.sdo_write_complete(0x1608, 0, &blob).await; + let blob = bridge::twin_layout_blob(0x6000); + let _ = sd.sdo_write_complete(0x1A08, 0, &blob).await; + let blob = bridge::assign_blob(&[0x1608]); + let _ = sd.sdo_write_complete(0x1C12, 0, &blob).await; + let blob = bridge::assign_blob(&[0x1A08]); + let _ = sd.sdo_write_complete(0x1C13, 0, &blob).await; + + // Configure SM2/SM3 with ESI values + FMMU + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x00u8).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_START) + .send(&maindevice, 0x1C00u16).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_LEN) + .send(&maindevice, 1024u16).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_CTRL) + .send(&maindevice, 0x64u8).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x01u8).await; + + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x00u8).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_START) + .send(&maindevice, 0x8E00u16).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_LEN) + .send(&maindevice, 1024u16).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_CTRL) + .send(&maindevice, 0x20u8).await; + let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) + .send(&maindevice, 0x01u8).await; + + bridge::write_fmmus_secondary(&maindevice, station).await; + + // SAFEOP let (al, alc) = bridge::drive_state(&maindevice, station, 0x0004).await; - println!(" {}", bridge::verdict(al, alc)); + println!(" SAFEOP: {}", bridge::verdict(al, alc)); if al & 0x0F != 0x04 || al & 0x10 != 0 { - return Err(format!("SAFEOP rejected: {}", bridge::verdict(al, alc)).into()); + return Err(format!("Cannot reach SAFEOP: {}", bridge::verdict(al, alc)).into()); } - - println!("Requesting OP..."); - let (al, alc) = bridge::drive_state(&maindevice, station, 0x0008).await; - println!(" {}", bridge::verdict(al, alc)); - if al & 0x0F != 0x08 || al & 0x10 != 0 { - return Err(format!("OP rejected: {}", bridge::verdict(al, alc)).into()); + println!(" >>> SAFEOP achieved! <<<"); + let (al2, alc2) = bridge::drive_state(&maindevice, station, 0x0008).await; + println!(" OP: {}", bridge::verdict(al2, alc2)); + if al2 & 0x0F != 0x08 || al2 & 0x10 != 0 { + return Err(format!("OP rejected: {}", bridge::verdict(al2, alc2)).into()); } + println!(" >>> OP achieved! <<<"); // ---- Optional EL2202 oscilloscope waveform (toggle one DO per cycle) ---- - // Simple DO terminals have no mailbox; their single output SyncManager - // (SM0) is NOT pre-programmed (ethercrab programs SMs/FMMUs only during - // the group PREOP->SAFEOP transition, which we deliberately skip for the - // bridge). Program SM0 manually per the EL2xxx family layout (outputs at - // physical 0x0F00, 1 byte, buffered ECAT-write ctrl 0x44), drive the - // terminal to OP, and verify the path with a write/readback before - // enabling the per-cycle toggle. const EL2202_OUT_ADDR: u16 = 0x0F00; - let mut el2202: Option<(u16, u16)> = None; // (station, output physical addr) + let mut el2202: Option<(u16, u16)> = None; if cfg.el2202 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { @@ -437,10 +381,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { let sd2 = group.subdevice(&maindevice, idx)?; let st2 = sd2.configured_address(); println!("[{}] Found EL2202, station 0x{:04X}", idx, st2); - // SM0: start/len/ctrl are complete config values; the - // enable byte is read-modify-write (set enable + the family - // bit per EEPROM — without the family bit the output stage - // stays inert and the pin does not switch). let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_START) .send(&maindevice, EL2202_OUT_ADDR) .await; @@ -465,7 +405,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 { return Err(format!("EL2202 OP rejected: {}", bridge::verdict(al, alc)).into()); } - // Verify the output path: write a byte, read it back. let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR) .send(&maindevice, regs::EL2202_CH1_DATA) .await; @@ -483,10 +422,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { .await; el2202 = Some((st2, EL2202_OUT_ADDR)); } else { - println!( - "EL2202 output readback failed (0x{:02X}); scope output disabled", - rb - ); + println!("EL2202 output readback failed (0x{:02X}); scope output disabled", rb); } } } @@ -499,14 +435,8 @@ async fn run(cfg: &Config) -> Result<(), Box> { } // ---- Optional EL2262 DC-locked reference output ---- - // The XFC terminal applies process data at its SYNC events, so with the - // DC sync unit programmed to the 1 ms cycle the output edges land on the - // DC grid regardless of frame arrival jitter — an oscilloscope (or the - // EL1252 below) can compare them against the loop-driven EL2202 edges. - // SM0 (outputs @0x1000, ctrl 0x64, enable 0x09 per EEPROM) is programmed - // manually for the same reason as the EL2202. const EL2262_OUT_ADDR: u16 = 0x1000; - let mut el2262: Option = None; // station + let mut el2262: Option = None; if cfg.el2262 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { @@ -515,10 +445,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { let sd3 = group.subdevice(&maindevice, idx)?; let st3 = sd3.configured_address(); println!("[{}] Found EL2262, station 0x{:04X}", idx, st3); - // DC sync unit: SYNC0+SYNC1 at the configured cycle, first - // pulse 100 ms in the future (DC system time is already - // running on these terminals). Start/cycle times are full - // values; the activation byte is read-modify-write. let t = ethercrab::Command::fprd(st3, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await @@ -541,12 +467,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { 0, ) .await; - println!( - "EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})", - cycle_ns, t - ); - // SM0: outputs @0x1000, 1 byte, ESI ctrl; enable via RMW - // (enable + family bit per EEPROM). + println!("EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})", cycle_ns, t); let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_START) .send(&maindevice, EL2262_OUT_ADDR) .await; @@ -580,12 +501,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } // ---- Optional EL1252 DC latch timestamp readback ---- - // The latch status/timestamps live in plain ESC registers (0x09AE - // status, 0x09B0 Latch0Pos, 0x09B8 Latch0Neg for channel 1). Edge - // latching only runs while the terminal is in OP, so configure its - // input SM0 (@0x1000, 1 byte, per EEPROM ctrl 0x22/enable 0x01) and - // drive it up — this also makes the raw input level byte readable. - let mut el1252: Option = None; // station + let mut el1252: Option = None; if cfg.el1252 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { @@ -627,14 +543,12 @@ async fn run(cfg: &Config) -> Result<(), Box> { } // ---- Pre-allocate everything, then promote to RT ---- - let mut tx_buf = vec![0u8; SM2_LEN]; - let mut rx_buf = vec![0u8; SM3_CAP]; - // Jitter: 100 ns bins, window +-5 ms. Exec/RTT: 1 us bins up to 20 ms. + let mut tx_buf = vec![0u8; SM_TX_LEN]; + let mut rx_buf = vec![0u8; SM_RX_LEN]; let mut jitter_st = OnlineStats::new("period_jitter", 100, -5_000_000, 5_000_000); let mut exec_st = OnlineStats::new("exec_time", 1_000, 0, 20_000_000); let mut rx_rtt_st = OnlineStats::new("fprd_rtt", 1_000, 0, 20_000_000); let mut tx_rtt_st = OnlineStats::new("fpwr_rtt", 1_000, 0, 20_000_000); - let mut phase_st = OnlineStats::new("phase_err", 100, -5_000_000, 5_000_000); println!("Promoting to RT: core={}, fifo={}, mlock=true", cfg.core, cfg.prio); if let Err(e) = rt::promote_current_thread(cfg.core, cfg.prio, true) { @@ -645,21 +559,19 @@ async fn run(cfg: &Config) -> Result<(), Box> { // ---- Real-time data loop ---- println!( "\n>>> running: {} loop, echo payload, stats (Ctrl-C for graceful stop)", - if follow { "follow-TwinCAT" } else { "ticker" } + match cfg.mode { + Mode::Ticker => "ticker", + Mode::Probe => "probe (read-only, no echo)", + } ); let start = Instant::now(); let mut ticker = rt::Ticker::new(cycle_ns); - let mut pll = Follow::new(cycle_ns, cfg.phase_us * 1000); - let mut last_ext_ts: Option = None; - let mut debug_ts = cfg.debug_ts; let mut cyc: u64 = 0; let mut tx_err: u64 = 0; let mut rx_err: u64 = 0; let mut err_streak: u64 = 0; let mut no_tc_seq: u64 = 0; - let mut ts_stale: u64 = 0; - let mut ts_rejected: u64 = 0; let mut el2202_err: u64 = 0; let mut el2262_err: u64 = 0; let mut el1252_err: u64 = 0; @@ -672,8 +584,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { let mut pos1_edges: u64 = 0; let mut el1252_st: [u8; 2] = [0, 0]; let mut el1252_in: u8 = 0; - // EL1252 edge stats per channel: interval between rising edges (1 us - // bins to 20 ms), and edge phase within the 1 ms cycle (100 ns bins). let mut latch_interval_st = OnlineStats::new("el1252_ch1_edge_interval", 1_000, 0, 20_000_000); let mut latch_phase_st = OnlineStats::new("el1252_ch1_edge_phase", 100, 0, 1_000_000); let mut latch1_interval_st = OnlineStats::new("el1252_ch2_edge_interval", 1_000, 0, 20_000_000); @@ -690,24 +600,21 @@ async fn run(cfg: &Config) -> Result<(), Box> { break; } - let jitter_ns = match cfg.mode { - Mode::Ticker | Mode::Probe => ticker.wait_next_period(), - Mode::Follow => pll.wait_next(), - }; + let jitter_ns = ticker.wait_next_period(); let t0 = Instant::now(); cyc += 1; jitter_st.push(jitter_ns); - // 1. Read SM3 (TwinCAT outputs, plus diag/timestamps) + // 1. Read SM1 (TwinCAT data) let t_rx0 = Instant::now(); let mut rx_ok = true; - match ethercrab::Command::fprd(station, SM3_ADDR) - .receive_slice(&maindevice, sm3_len as u16) + match ethercrab::Command::fprd(station, SM_RX_ADDR) + .receive_slice(&maindevice, SM_RX_LEN as u16) .await { Ok(pdu) => { let raw: &[u8] = &pdu; - let n = raw.len().min(sm3_len); + let n = raw.len().min(SM_RX_LEN); rx_buf[..n].copy_from_slice(&raw[..n]); } Err(_) => { @@ -716,104 +623,29 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } rx_rtt_st.push(t_rx0.elapsed().as_nanos() as i64); - let read_ns = rt::now_ns(); - // 2. Timestamp handling. - if rx_ok && ts_layout { - let ext_ts = u64::from_le_bytes( - rx_buf[TS_EXT_OFF..TS_EXT_OFF + 8].try_into().unwrap_or([0; 8]), - ); - if cfg.mode == Mode::Probe { - // No feedback: just dump the ext phase and inter-sample delta. - if ext_ts == 0 { - ts_stale += 1; - } else { - let d = last_ext_ts.map(|l| ext_ts.wrapping_sub(l)); - if Some(ext_ts) != last_ext_ts { - if debug_ts > 0 && cyc > 2000 { - println!( - "TS cyc={} ext_mod_us={} d={} read_mod_us={}", - cyc, - ext_ts % cycle_ns / 1000, - d.unwrap_or(0) as i64, - read_ns % cycle_ns / 1000, - ); - debug_ts -= 1; - } - last_ext_ts = Some(ext_ts); - } else { - ts_stale += 1; - } - } - } else if ext_ts == 0 { - ts_stale += 1; - } else { - // Plausibility is judged against the previous *raw* sample, - // so a single bad sample causes at most two rejects and the - // filter re-syncs itself (comparing against the last accepted - // sample instead would deadlock after one startup outlier). - let d = last_ext_ts.map(|l| ext_ts.wrapping_sub(l)); - last_ext_ts = Some(ext_ts); - let verdict = match d { - None => "accept", - Some(0) => "stale", - Some(dd) if dd <= TS_MAX_GAP_NS => "accept", - Some(_) => "reject", - }; - if debug_ts > 0 && cyc > 2000 { - println!( - "TS cyc={} ext_mod_us={} d={} off={} wait={} anchor={} dl_mod_us={} {}", - cyc, - ext_ts % cycle_ns / 1000, - d.unwrap_or(0) as i64, - pll.offset_ns, - pll.last_wait_ns, - pll.next_target_tc, - (pll.next_target_tc as i64 + pll.offset_ns) - .rem_euclid(cycle_ns as i64) - / 1000, - verdict - ); - debug_ts -= 1; - } - match verdict { - "accept" => { - pll.update(ext_ts, read_ns); - let target = pll.current_target_local(ext_ts); - phase_st.push(read_ns as i64 - target as i64); - } - "stale" => ts_stale += 1, - _ => ts_rejected += 1, - } + // 2. In ticker mode: echo payload back and stamp cycle number. + // In probe mode: skip TX (read-only observation). + if cfg.mode == Mode::Ticker { + if rx_ok { + let n = PAYLOAD_LEN.min(rx_buf.len()); + tx_buf[..n].copy_from_slice(&rx_buf[..n]); + } + tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes()); + tx_buf[264..268].copy_from_slice(&(jitter_ns as i32).to_le_bytes()); + + let t_tx0 = Instant::now(); + let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR) + .send(&maindevice, &tx_buf[..PAYLOAD_LEN]) + .await + .is_ok(); + tx_rtt_st.push(t_tx0.elapsed().as_nanos() as i64); + if !tx_ok { + tx_err += 1; } } - // 3. Echo TwinCAT's payload straight back, stamp our cycle number at - // bytes 8..16 (TwinCAT round-trip check uses bytes 0..8). - if rx_ok { - let data = &rx_buf[data_off..]; - let n = PAYLOAD_LEN.min(data.len()); - tx_buf[..n].copy_from_slice(&data[..n]); - } - tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes()); - - let t_tx0 = Instant::now(); - let tx_ok = ethercrab::Command::fpwr(station, SM2_ADDR) - .send(&maindevice, &tx_buf[..PAYLOAD_LEN]) - .await - .is_ok(); - tx_rtt_st.push(t_tx0.elapsed().as_nanos() as i64); - if !tx_ok { - tx_err += 1; - } - - // Optional EL2202 scope waveform: toggle one output every cycle - // (dual mode: both channels). The EL2202 process byte is - // [bit0=ch1 data, bit1=ch1 TRISTATE, bit2=ch2 data, bit3=ch2 - // TRISTATE] (PDO 0x1600/0x1601, verified from EEPROM) — writing - // ch1 data + ch1 tristate puts channel 1 into high-resistance, so - // dual mode must only set the two DATA bits. This is a process-data - // payload (the whole byte is ours), not a register bit modification. + // Optional EL2202 scope waveform if let Some((st2, addr)) = el2202 { let v: u8 = if cfg.el2202_dual { (cyc & 1) as u8 * (regs::EL2202_CH1_DATA | regs::EL2202_CH2_DATA) @@ -829,8 +661,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } - // Optional EL2262 DC-locked reference output: toggle every cycle; - // the terminal applies the new value at its next SYNC event. + // Optional EL2262 DC-locked reference output if let Some(st3) = el2262 { let v: u8 = (cyc & 1) as u8; if ethercrab::Command::fpwr(st3, EL2262_OUT_ADDR) @@ -842,12 +673,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } - // Optional EL1252 latch readback: 34 bytes @0x09AE = - // [status0, status1, Latch0Pos u64, Latch0Neg u64, - // Latch1Pos u64, Latch1Neg u64] (PDO 0x1A13 extent, ESC registers - // 0x09AE..0x09CF). A changed LatchXPos means a new rising edge was - // captured on that channel; intervals between rising edges and - // their phase within the cycle are the jitter statistics. + // Optional EL1252 latch readback if let Some(st4) = el1252 { if cyc % (cfg.el1252_every.max(1) as u64) == 0 { match ethercrab::Command::fprd(st4, 0x09AEu16) @@ -892,8 +718,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } - // Error-streak guard: a dead link must abort loudly, not accumulate. - if rx_ok && tx_ok { + if rx_ok && (cfg.mode == Mode::Probe || !tx_err > 0) { err_streak = 0; } else { err_streak += 1; @@ -906,7 +731,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } let tc_seq = u64::from_le_bytes( - rx_buf[data_off..data_off + 8].try_into().unwrap_or([0; 8]), + rx_buf[..8].try_into().unwrap_or([0; 8]), ); if tc_seq == 0 { no_tc_seq += 1; @@ -915,7 +740,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { exec_st.push(t0.elapsed().as_nanos() as i64); if !cfg.quiet && last_report.elapsed() >= Duration::from_millis(1000) { - // Diagnostic: raw EL1252 input level byte (SM0 @0x1000), 1 Hz only. if let Some(st4) = el1252 { if let Ok(b) = ethercrab::Command::fprd(st4, 0x1000u16) .receive::(&maindevice) @@ -924,20 +748,14 @@ async fn run(cfg: &Config) -> Result<(), Box> { el1252_in = b; } } - let diag = u16::from_le_bytes([rx_buf[0], rx_buf[1]]); println!( - "S tms={} cyc={} diag=0x{:04X} tc_seq={} off={} wait={} txe={} rxe={} notc={} ts0={} tsrej={} e0={} e1={} in=0x{:02X} lst={:02X}{:02X}", + "S tms={} cyc={} tc_seq={} txe={} rxe={} notc={} e0={} e1={} in=0x{:02X} lst={:02X}{:02X}", start.elapsed().as_millis(), cyc, - diag, tc_seq, - pll.offset_ns, - pll.last_wait_ns, tx_err, rx_err, no_tc_seq, - ts_stale, - ts_rejected, pos_edges, pos1_edges, el1252_in, @@ -958,10 +776,6 @@ async fn run(cfg: &Config) -> Result<(), Box> { exec_st.report(1000, "us"); rx_rtt_st.report(1000, "us"); tx_rtt_st.report(1000, "us"); - if follow { - println!("=== phase error: SM3 read point vs (TC grid boundary + phase) ==="); - phase_st.report(1000, "us"); - } if el1252.is_some() { println!("=== EL1252 latch: rising-edge intervals and phase in cycle ==="); latch_interval_st.report(1000, "us"); @@ -974,14 +788,12 @@ async fn run(cfg: &Config) -> Result<(), Box> { ); } println!( - "\n=== done: {} cycles in {:.1}s, tx_err={}, rx_err={}, no_tc_seq={}, ts_stale={}, ts_rejected={}, el2202_err={}, el2262_err={} ===", + "\n=== done: {} cycles in {:.1}s, tx_err={}, rx_err={}, no_tc_seq={}, el2202_err={}, el2262_err={} ===", cyc, start.elapsed().as_secs_f64(), tx_err, rx_err, no_tc_seq, - ts_stale, - ts_rejected, el2202_err, el2262_err ); diff --git a/src/regs.rs b/src/regs.rs index c908930..7a6f5b8 100644 --- a/src/regs.rs +++ b/src/regs.rs @@ -42,8 +42,10 @@ pub const SM_CTRL_DIR_ECAT_WRITE: u8 = 0x04; // Full ctrl bytes verified from each device's EEPROM SM category. These are // complete configuration values (mode + direction + vendor bits), written // wholesale during initialization — not bit modifications. -pub const SM_CTRL_OUTPUTS_ESI: u8 = 0x64; // EL6695 SM2, EL2262 SM0 -pub const SM_CTRL_INPUTS_ESI: u8 = 0x20; // EL6695 SM3 +pub const SM_CTRL_OUTPUTS_ESI: u8 = 0x64; // EL6695 SM2 (primary), EL2262 SM0 +pub const SM_CTRL_INPUTS_ESI: u8 = 0x20; // EL6695 SM3 (primary) +pub const SM_CTRL_OUTPUTS_SEC: u8 = 0x70; // EL6695 SM0 (secondary, buffered WRITE output) +pub const SM_CTRL_INPUTS_SEC: u8 = 0x20; // EL6695 SM1 (secondary, buffered input, matching primary SM3) pub const SM_CTRL_EL2202_OUTPUTS: u8 = 0x44; // EL2202 SM0 pub const SM_CTRL_EL1252_INPUTS: u8 = 0x22; // EL1252 SM0