//! 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: //! //! - `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)]. //! //! 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] use std::sync::atomic::{AtomicBool, Ordering}; use std::time::{Duration, Instant}; use el6695_rt::{bridge, regs, rt, stats}; use ethercrab::{ std::{ethercat_now, tx_rx_task}, MainDevice, MainDeviceConfig, PduStorage, Timeouts, }; use stats::OnlineStats; const MAX_SUBDEVICES: usize = 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; 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> = std::sync::OnceLock::new(); extern "C" fn on_signal(_sig: libc::c_int) { STOP.store(true, Ordering::Relaxed); } #[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, } 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, /// Toggle BOTH EL2202 channels (0x03/0x00): output 1 -> EL1252 readback, /// output 2 -> oscilloscope, same edges in two views. el2202_dual: bool, /// Read EL1252 DC latch timestamps every cycle (edge interval/phase /// statistics; wire the signal under test to EL1252 channel 1). el1252: bool, /// Drive an EL2262 XFC output with DC-sync-locked edges (reference /// square wave on the DC 1 ms grid). el2262: bool, /// Read the EL1252 every N cycles (frame-budget throttle when combined /// with other per-cycle frames; edges are latched so nothing is lost /// beyond time resolution). el1252_every: u32, } fn parse_args() -> Result { let mut cfg = Config { iface: "eth1".into(), mode: Mode::Follow, 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, el2262: false, 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(); } } while let Some(a) = args.next() { let mut val = |name: &str| -> Result { args.next().ok_or_else(|| format!("missing value for {}", name)) }; match a.as_str() { "--mode" => { cfg.mode = match val("--mode")?.as_str() { "ticker" => Mode::Ticker, "follow" => Mode::Follow, "probe" => Mode::Probe, other => return Err(format!("unknown mode '{}'", other)), } } "--duration" => { cfg.duration_sec = val("--duration")? .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")?, "--quiet" => cfg.quiet = true, "--el2202" => cfg.el2202 = true, "--el2202-dual" => { cfg.el2202 = true; cfg.el2202_dual = true; } "--el1252" => cfg.el1252 = true, "--el2262" => cfg.el2262 = true, "--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)), } } if cfg.cycle_us == 0 { return Err("--cycle-us must be > 0".into()); } 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]" ); 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 libc::sigemptyset(&mut sa.sa_mask); libc::sigaction(libc::SIGINT, &sa, core::ptr::null_mut()); libc::sigaction(libc::SIGTERM, &sa, core::ptr::null_mut()); } let code = match futures_lite::future::block_on(run(&cfg)) { Ok(()) => 0, Err(e) => { eprintln!("Failed: {}", e); 1 } }; std::process::exit(code); } 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!( " iface={} mode={} duration={}s cycle={}us phase={}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")?; let maindevice = MainDevice::new( pdu_loop, Timeouts { state_transition: Duration::from_millis(10000), mailbox_response: Duration::from_millis(2000), eeprom: Duration::from_millis(500), ..Default::default() }, MainDeviceConfig::default(), ); let iface = cfg.iface.clone(); let _txrx = std::thread::spawn(move || { let task = tx_rx_task(&iface, tx, rx).expect("create tx_rx task"); if let Err(e) = futures_lite::future::block_on(task) { eprintln!("tx_rx task ended with error: {:?}", e); } }); println!("Scanning segment (INIT -> PREOP)..."); let group = maindevice .init_single_group::(ethercat_now) .await?; let mut el6695_index = None; for (idx, subdevice) in group.iter(&maindevice).enumerate() { if subdevice.name().contains("EL6695") { el6695_index = Some(idx); println!("[{}] Found EL6695", idx); } } let el6695_index = el6695_index.ok_or("EL6695 not found on bus")?; let sd = group.subdevice(&maindevice, el6695_index)?; 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), } } // ---- OD self-check / self-config ---- let txpdo_first: u16 = if ts_layout { 0x1A02 } else { 0x1A01 }; bridge::ensure_od(&sd, &maindevice, station, txpdo_first) .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..."); let (al, alc) = bridge::drive_state(&maindevice, station, 0x0004).await; println!(" {}", bridge::verdict(al, alc)); if al & 0x0F != 0x04 || al & 0x10 != 0 { return Err(format!("SAFEOP rejected: {}", 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()); } // ---- 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) if cfg.el2202 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { if subdevice.name().contains("EL2202") { found = true; 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; let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_LEN) .send(&maindevice, 1u16) .await; let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_CTRL) .send(&maindevice, regs::SM_CTRL_EL2202_OUTPUTS) .await; let _ = regs::rmw_u8( &maindevice, st2, regs::sm_base(0) + regs::SM_OFF_ENABLE, regs::SM_ENABLE | regs::SM_ENABLE_FAMILY, 0, ) .await; let (al, alc) = bridge::drive_state(&maindevice, st2, regs::AL_SAFEOP).await; println!("EL2202 SAFEOP: {}", bridge::verdict(al, alc)); let (al, alc) = bridge::drive_state(&maindevice, st2, regs::AL_OP).await; println!("EL2202 OP: {}", bridge::verdict(al, alc)); 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; let rb = ethercrab::Command::fprd(st2, EL2202_OUT_ADDR) .receive::(&maindevice) .await .unwrap_or(0xFF); if rb & regs::EL2202_CH1_DATA == regs::EL2202_CH1_DATA { println!( "EL2202 scope output: verified, toggle 1 byte @0x{:04X} every cycle", EL2202_OUT_ADDR ); let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR) .send(&maindevice, 0x00u8) .await; el2202 = Some((st2, EL2202_OUT_ADDR)); } else { println!( "EL2202 output readback failed (0x{:02X}); scope output disabled", rb ); } } } if !found { println!("--el2202 given but no EL2202 on bus; scope output disabled"); } if found && el2202.is_none() { println!("EL2202 present but output setup failed; scope output disabled"); } } // ---- 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 if cfg.el2262 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { if subdevice.name().contains("EL2262") { found = true; 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 .unwrap_or(0); let start = t.wrapping_add(100_000_000); let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC_START_TIME) .send(&maindevice, start) .await; let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC0_CYCLE) .send(&maindevice, cycle_ns as u32) .await; let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC1_CYCLE) .send(&maindevice, cycle_ns as u32) .await; let _ = regs::rmw_u8( &maindevice, st3, regs::DC_SYNC_ACTIVATION, regs::DC_SYNC0_ACTIVATE | regs::DC_SYNC1_ACTIVATE, 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). let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_START) .send(&maindevice, EL2262_OUT_ADDR) .await; let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_LEN) .send(&maindevice, 1u16) .await; let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_CTRL) .send(&maindevice, regs::SM_CTRL_OUTPUTS_ESI) .await; let _ = regs::rmw_u8( &maindevice, st3, regs::sm_base(0) + regs::SM_OFF_ENABLE, regs::SM_ENABLE | regs::SM_ENABLE_FAMILY, 0, ) .await; let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_SAFEOP).await; println!("EL2262 SAFEOP: {}", bridge::verdict(al, alc)); let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_OP).await; println!("EL2262 OP: {}", bridge::verdict(al, alc)); if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 { return Err(format!("EL2262 OP rejected: {}", bridge::verdict(al, alc)).into()); } el2262 = Some(st3); } } if !found { println!("--el2262 given but no EL2262 on bus; reference output disabled"); } } // ---- 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 if cfg.el1252 { let mut found = false; for (idx, subdevice) in group.iter(&maindevice).enumerate() { if subdevice.name().contains("EL1252") { found = true; let sd4 = group.subdevice(&maindevice, idx)?; let st4 = sd4.configured_address(); println!("[{}] Found EL1252, station 0x{:04X} (latch readback)", idx, st4); let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_START) .send(&maindevice, 0x1000u16) .await; let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_LEN) .send(&maindevice, 1u16) .await; let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_CTRL) .send(&maindevice, regs::SM_CTRL_EL1252_INPUTS) .await; let _ = regs::rmw_u8( &maindevice, st4, regs::sm_base(0) + regs::SM_OFF_ENABLE, regs::SM_ENABLE, 0, ) .await; let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_SAFEOP).await; println!("EL1252 SAFEOP: {}", bridge::verdict(al, alc)); let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_OP).await; println!("EL1252 OP: {}", bridge::verdict(al, alc)); if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 { return Err(format!("EL1252 OP rejected: {}", bridge::verdict(al, alc)).into()); } el1252 = Some(st4); } } if !found { println!("--el1252 given but no EL1252 on bus; latch readback disabled"); } } // ---- 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 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) { eprintln!("RT promotion failed (needs root, continuing anyway): {}", e); } rt::prefault_stack(256 * 1024); // ---- Real-time data loop ---- println!( "\n>>> running: {} loop, echo payload, stats (Ctrl-C for graceful stop)", if follow { "follow-TwinCAT" } else { "ticker" } ); 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; let mut latch_pos0: u64 = 0; let mut latch_neg0: u64 = 0; let mut latch_pos1: u64 = 0; let mut latch_neg1: u64 = 0; let mut pos_edges: u64 = 0; let mut neg_edges: u64 = 0; 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); let mut latch1_phase_st = OnlineStats::new("el1252_ch2_edge_phase", 100, 0, 1_000_000); let mut last_report = Instant::now(); let mut aborted: Option = None; loop { if STOP.load(Ordering::Relaxed) { println!("\n(stop requested, finishing cycle {})", cyc); break; } if cfg.duration_sec > 0 && start.elapsed().as_secs() >= cfg.duration_sec { break; } let jitter_ns = match cfg.mode { Mode::Ticker | Mode::Probe => ticker.wait_next_period(), Mode::Follow => pll.wait_next(), }; let t0 = Instant::now(); cyc += 1; jitter_st.push(jitter_ns); // 1. Read SM3 (TwinCAT outputs, plus diag/timestamps) let t_rx0 = Instant::now(); let mut rx_ok = true; match ethercrab::Command::fprd(station, SM3_ADDR) .receive_slice(&maindevice, sm3_len as u16) .await { Ok(pdu) => { let raw: &[u8] = &pdu; let n = raw.len().min(sm3_len); rx_buf[..n].copy_from_slice(&raw[..n]); } Err(_) => { rx_ok = false; rx_err += 1; } } 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, } } } // 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. 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) } else { (cyc & 1) as u8 * regs::EL2202_CH1_DATA }; if ethercrab::Command::fpwr(st2, addr) .send(&maindevice, v) .await .is_err() { el2202_err += 1; } } // Optional EL2262 DC-locked reference output: toggle every cycle; // the terminal applies the new value at its next SYNC event. if let Some(st3) = el2262 { let v: u8 = (cyc & 1) as u8; if ethercrab::Command::fpwr(st3, EL2262_OUT_ADDR) .send(&maindevice, v) .await .is_err() { el2262_err += 1; } } // 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. if let Some(st4) = el1252 { if cyc % (cfg.el1252_every.max(1) as u64) == 0 { match ethercrab::Command::fprd(st4, 0x09AEu16) .receive_slice(&maindevice, 34) .await { Ok(pdu) => { let e: &[u8] = &pdu; if e.len() >= 34 { el1252_st = [e[0], e[1]]; let pos0 = u64::from_le_bytes(e[2..10].try_into().unwrap()); let neg0 = u64::from_le_bytes(e[10..18].try_into().unwrap()); let pos1 = u64::from_le_bytes(e[18..26].try_into().unwrap()); let neg1 = u64::from_le_bytes(e[26..34].try_into().unwrap()); if pos0 != 0 && pos0 != latch_pos0 { if latch_pos0 != 0 { latch_interval_st.push((pos0 - latch_pos0) as i64); } latch_phase_st.push((pos0 % cycle_ns) as i64); latch_pos0 = pos0; pos_edges += 1; } if neg0 != 0 && neg0 != latch_neg0 { latch_neg0 = neg0; neg_edges += 1; } if pos1 != 0 && pos1 != latch_pos1 { if latch_pos1 != 0 { latch1_interval_st.push((pos1 - latch_pos1) as i64); } latch1_phase_st.push((pos1 % cycle_ns) as i64); latch_pos1 = pos1; pos1_edges += 1; } if neg1 != 0 && neg1 != latch_neg1 { latch_neg1 = neg1; } } } Err(_) => el1252_err += 1, } } } // Error-streak guard: a dead link must abort loudly, not accumulate. if rx_ok && tx_ok { err_streak = 0; } else { err_streak += 1; if err_streak > MAX_ERR_STREAK { aborted = Some(format!( "aborting: {} consecutive cycles with TX/RX errors (link dead?)", err_streak )); } } let tc_seq = u64::from_le_bytes( rx_buf[data_off..data_off + 8].try_into().unwrap_or([0; 8]), ); if tc_seq == 0 { no_tc_seq += 1; } 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) .await { 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}", 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, el1252_st[0], el1252_st[1] ); last_report = Instant::now(); } if aborted.is_some() { break; } } // ---- Final report ---- println!("\n=== timing stats ==="); jitter_st.report(1000, "us"); 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"); latch_phase_st.report(1000, "us"); latch1_interval_st.report(1000, "us"); latch1_phase_st.report(1000, "us"); println!( "el1252: ch1 pos_edges={}, ch1 neg_edges={}, ch2 pos_edges={}, read_err={}", pos_edges, neg_edges, pos1_edges, el1252_err ); } println!( "\n=== done: {} cycles in {:.1}s, tx_err={}, rx_err={}, no_tc_seq={}, ts_stale={}, ts_rejected={}, 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 ); if let Some(msg) = aborted { return Err(msg.into()); } Ok(()) }