From 49de477028a3a96947e5ad5e2b84ed595d4c21ba Mon Sep 17 00:00:00 2001 From: Tony Cao Date: Mon, 26 Oct 2026 15:59:21 +0800 Subject: [PATCH] feat(sync-ts): DC follow experiment tooling + Rev6 findings - --sync-ts mode: secondary 0x1A02 timestamp PDO + 0x10F5:01 sync-master select + 0x0920 DC offset init (manual section 6.7 direct-coupling recipe) - Rev6 empirical matrix: secondary accepts only compact 0x1A01 (with pre-SAFEOP DC writes); timestamp 0x1A02 (22B) rejected with AL 0x1E 'invalid INPUT config' under all combinations -> July-style timestamp PLL blocked on Rev6 secondary side (firmware limitation, hotline item) - manual v1.9.0 says DC regs 0x0980+ don't exist; Rev6 accepts and requires them for SYNC PDO at SAFEOP - kept, gated on prefix --- src/main.rs | 38 ++++++++++++++++++++++++++++++++------ 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/src/main.rs b/src/main.rs index 3d3bd2b..e381713 100644 --- a/src/main.rs +++ b/src/main.rs @@ -126,6 +126,7 @@ struct Config { sii_fix: bool, /// 完全采纳从站 EEPROM 默认配置, 不写任何 SM/FMMU/PDO adopt_pure: bool, + sync_ts: bool, /// 强制 SM3 长度 (0=默认 PAYLOAD_LEN+sm3_prefix) sm3_len_force: u16, /// Enable DC SYNC0 on the EL6695 secondary (cycle = --cycle-us) and print @@ -157,6 +158,7 @@ fn parse_args() -> Result { f800_set: false, sii_fix: false, adopt_pure: false, + sync_ts: false, sm3_len_force: 0, dc: false, iouring: false, @@ -202,6 +204,7 @@ fn parse_args() -> Result { "--f800-set" => cfg.f800_set = true, "--sii-fix" => cfg.sii_fix = true, "--adopt-pure" => cfg.adopt_pure = true, + "--sync-ts" => cfg.sync_ts = true, "--sm3-len" => { cfg.sm3_len_force = val("--sm3-len")?.parse().map_err(|_| "bad --sm3-len")?; }, "--dc" => cfg.dc = true, "--iouring" => cfg.iouring = true, @@ -636,7 +639,28 @@ async fn run(cfg: &Config) -> Result<(), Box> { bytes } else { 0 } }; - let sm3_prefix: usize = 0; // FORCE plain layout: SYNC prefix breaks Rev6 reverse forwarding + let sm3_prefix: usize = if cfg.sync_ts { 22 } else { 0 }; // 0x1A02 timestamp prefix = 22 bytes + if cfg.sync_ts { + let blob13 = bridge::assign_blob(&[0x1A02, 0x1A08]); + match bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await { + Ok(_) => println!(" sync-ts: 0x1C13=[0x1A02,0x1A08] written"), + Err(e) => println!(" sync-ts: 0x1C13 write ERR {:?}", e), + } + // Manual: 10F5:01 0=SyncMaster on primary, 1=on secondary. Follower(D2000)=SyncSlave + // with CX(primary)=SyncMaster -> correct value is 0. Explicitly restore (may be persisted as 1). + let w = sd.sdo_write::(0x10F5, 1, 0).await; + let cur = sd.sdo_read::(0x10F5, 1).await.unwrap_or(0xFF); + println!(" sync-ts: 0x10F5:01 write0={:?} readback={}", w.is_ok(), cur); + // Manual §6.7 direct DC coupling: each master must write 0x0920 system-time-offset + // from its side; EL6695 then frequency-locks both ESCs. Align ESC time to local mono clock. + let dc_t = ethercrab::Command::fprd(station, 0x0910u16).receive::(&maindevice).await.unwrap_or(0); + let loc = rt::now_ns(); + let off = loc.wrapping_sub(dc_t); + let wo = ethercrab::Command::fpwr(station, 0x0920u16).send(&maindevice, off).await; + let dc_t2 = ethercrab::Command::fprd(station, 0x0910u16).receive::(&maindevice).await.unwrap_or(0); + let loc2 = rt::now_ns(); + println!(" sync-ts: 0x0920 offset write={:?}; now delta(local-esc)={}ns", wo.is_ok(), loc2.wrapping_sub(dc_t2) as i64); + } match cfg.role { Role::Consumer => { @@ -781,13 +805,15 @@ async fn run(cfg: &Config) -> Result<(), Box> { } // 新固件 SYNC PDO (0x1A01) 需要 DC SYNC0 已配置才接受 SAFEOP + // Rev6 firmware empirically REQUIRES these writes to accept a SYNC PDO at SAFEOP + // (manual §6.7 says these registers don't exist - true for old FW; Rev6 accepts them). if sm3_prefix > 0 { let t_now = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); let start_t = t_now.wrapping_add(200_000_000); let w1 = ethercrab::Command::fpwr(station, regs::DC_SYNC_START_TIME).send(&maindevice, start_t).await; let w2 = ethercrab::Command::fpwr(station, regs::DC_SYNC0_CYCLE).send(&maindevice, 1_000_000u32).await; let w3 = ethercrab::Command::fpwr(station, regs::DC_SYNC_ACTIVATION).send(&maindevice, regs::DC_SYNC0_ACTIVATE).await; - println!(" pre-SAFEOP DC: start={:?} cycle={:?} act={:?}", w1.is_ok(), w2.is_ok(), w3.is_ok()); + println!(" pre-SAFEOP DC(Rev6): start={:?} cycle={:?} act={:?}", w1.is_ok(), w2.is_ok(), w3.is_ok()); for idx in [0x1C32u16, 0x1C33] { match sd.sdo_write::(idx, 0x01, 2).await { Ok(_) => println!(" pre-SAFEOP: CoE 0x{:04X}:01 sync_mode=2 OK", idx), @@ -826,15 +852,15 @@ async fn run(cfg: &Config) -> Result<(), Box> { let n = sd.sdo_read::(0x1C13, 0).await.unwrap_or(0xFF); let first = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); println!(" runtime 0x1C13: count={} first=0x{:04X}", n, first); - if first == 0x1A01 { - let cnt = sd.sdo_read::(0x1A01, 0).await.unwrap_or(0); + if first == 0x1A01 || first == 0x1A02 { + let cnt = sd.sdo_read::(first, 0).await.unwrap_or(0); let mut bits = 0u32; for sub in 1..=cnt { - let e = sd.sdo_read::(0x1A01, sub).await.unwrap_or(0); + let e = sd.sdo_read::(first, sub).await.unwrap_or(0); bits += e & 0xFF; } let bytes = ((bits + 7) / 8) as usize; - println!(" SYNC prefix detected: 0x1A01 = {} bits = {} bytes", bits, bytes); + println!(" SYNC prefix detected: 0x{:04X} = {} bits = {} bytes", first, bits, bytes); bytes } else { 0 } };