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
This commit is contained in:
Tony Cao
parent
21b2d3bf99
commit
2837f2a13d
132
docs/verification-progress.md
Normal file
132
docs/verification-progress.md
Normal file
|
|
@ -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@<j1900-ip> /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`
|
||||
105
src/bridge.rs
105
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<u8> {
|
||||
pub fn twin_layout_blob(obj: u16) -> Vec<u8> {
|
||||
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<u8> {
|
|||
}
|
||||
|
||||
/// SM assign blob: entry count (u16 LE) + mapped object indices.
|
||||
fn assign_blob(objs: &[u16]) -> Vec<u8> {
|
||||
pub fn assign_blob(objs: &[u16]) -> Vec<u8> {
|
||||
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<E: std::fmt::Debug>(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<S>(
|
||||
sd: ðercrab::SubDeviceRef<'_, S>,
|
||||
md: &MainDevice<'_>,
|
||||
) -> Result<(), String>
|
||||
where
|
||||
S: std::ops::Deref<Target = ethercrab::SubDevice>,
|
||||
{
|
||||
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;
|
||||
|
|
|
|||
578
src/main.rs
578
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<PduStorage<MAX_FRAMES, MAX_PDU_DATA>> =
|
||||
|
|
@ -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<Config, String> {
|
||||
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<Config, String> {
|
|||
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<Config, String> {
|
|||
"--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<Config, String> {
|
|||
.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<Config, String> {
|
|||
"--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<Config, String> {
|
|||
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<dyn std::error::Error>> {
|
||||
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<dyn std::error::Error>> {
|
|||
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::<u16>(md).await.unwrap_or(0xFFFF);
|
||||
let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::<u16>(md).await.unwrap_or(0xFFFF);
|
||||
let ctrl = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_CTRL).receive::<u8>(md).await.unwrap_or(0xFF);
|
||||
let ena = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_ENABLE).receive::<u8>(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<dyn std::error::Error> { 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::<u8>(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::<u16>(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<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
.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<dyn std::error::Error>> {
|
|||
}
|
||||
|
||||
// ---- 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<u16> = None; // station
|
||||
let mut el2262: Option<u16> = 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<dyn std::error::Error>> {
|
|||
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::<u64>(&maindevice)
|
||||
.await
|
||||
|
|
@ -541,12 +467,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
}
|
||||
|
||||
// ---- 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<u16> = None; // station
|
||||
let mut el1252: Option<u16> = 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<dyn std::error::Error>> {
|
|||
}
|
||||
|
||||
// ---- 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<dyn std::error::Error>> {
|
|||
// ---- 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<u64> = 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<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
}
|
||||
}
|
||||
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<dyn std::error::Error>> {
|
|||
}
|
||||
}
|
||||
|
||||
// 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<dyn std::error::Error>> {
|
|||
}
|
||||
}
|
||||
|
||||
// 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<dyn std::error::Error>> {
|
|||
}
|
||||
}
|
||||
|
||||
// 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<dyn std::error::Error>> {
|
|||
}
|
||||
|
||||
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<dyn std::error::Error>> {
|
|||
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::<u8>(&maindevice)
|
||||
|
|
@ -924,20 +748,14 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
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<dyn std::error::Error>> {
|
|||
);
|
||||
}
|
||||
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
|
||||
);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user