994 lines
40 KiB
Rust
994 lines
40 KiB
Rust
|
//! 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<PduStorage<MAX_FRAMES, MAX_PDU_DATA>> =
|
||
|
|
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<Config, String> {
|
||
|
|
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<String, String> {
|
||
|
|
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<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!(
|
||
|
|
" 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::<MAX_SUBDEVICES, PDI_LEN>(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<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...");
|
||
|
|
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::<u8>(&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<u16> = 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::<u64>(&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<u16> = 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<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;
|
||
|
|
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<String> = 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::<u8>(&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(())
|
||
|
|
}
|