diff --git a/src/main.rs b/src/main.rs index e381713..0c6478c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -127,6 +127,7 @@ struct Config { /// 完全采纳从站 EEPROM 默认配置, 不写任何 SM/FMMU/PDO adopt_pure: bool, sync_ts: bool, + dc_discipline: bool, /// 强制 SM3 长度 (0=默认 PAYLOAD_LEN+sm3_prefix) sm3_len_force: u16, /// Enable DC SYNC0 on the EL6695 secondary (cycle = --cycle-us) and print @@ -159,6 +160,7 @@ fn parse_args() -> Result { sii_fix: false, adopt_pure: false, sync_ts: false, + dc_discipline: false, sm3_len_force: 0, dc: false, iouring: false, @@ -205,6 +207,7 @@ fn parse_args() -> Result { "--sii-fix" => cfg.sii_fix = true, "--adopt-pure" => cfg.adopt_pure = true, "--sync-ts" => cfg.sync_ts = true, + "--dc-discipline" => cfg.dc_discipline = true, "--sm3-len" => { cfg.sm3_len_force = val("--sm3-len")?.parse().map_err(|_| "bad --sm3-len")?; }, "--dc" => cfg.dc = true, "--iouring" => cfg.iouring = true, @@ -1554,7 +1557,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { // ---- rfm-async: event-driven forwarder, no tick ---- if cfg.mode == Mode::RfmAsync { let rfm = rfm_ctx.take().expect("rfm not opened"); - return rfm_async_loop(&maindevice, station, rfm, cfg.duration_sec).await; + return rfm_async_loop(&maindevice, station, rfm, cfg.duration_sec, cfg.dc_discipline).await; } loop { @@ -2316,6 +2319,7 @@ async fn rfm_async_loop( station: u16, mut rfm: el6695_rt::rfm::Rfm, duration_sec: u64, + dc_discipline: bool, ) -> Result<(), Box> { use el6695_rt::rfm::*; println!("rfm-async: event-driven forwarding, no tick (Ctrl-C to stop)"); @@ -2373,10 +2377,49 @@ async fn rfm_async_loop( let mut seg_fpwr_us: u64 = 0; let mut seg_rpt_us: u64 = 0; + // IgH-style continuous DC discipline (manual 6.7 direct coupling) + let mut dc_prev: Option<(u64, u64)> = None; // (esc_time_ns, local_ns) + let mut dc_first: Option<(u64, u64)> = None; + let mut dc_off_applied: u64 = 0; + let mut dc_last_tick: Option = None; + let mut dc_err_ns: i64 = 0; + let mut dc_drift_ppm: f64 = 0.0; + let mut phase_st = OnlineStats::new("fresh_phase_us", 200, -1_500_000, 1_500_000); + let mut phase_est: i64 = 0; // estimated TwinCAT frame boundary phase (ns within 1ms) + let mut wphase_st = OnlineStats::new("write_phase_us", 200, -1_500_000, 1_500_000); + while !STOP.load(Ordering::Relaxed) { if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { break; } + // DC discipline tick (every ~10ms): align ESC time to local clock + if dc_discipline { + let tick = dc_last_tick.map(|t| t.elapsed().as_millis() as u64).unwrap_or(999); + if tick >= 10 { + dc_last_tick = Some(std::time::Instant::now()); + if let Ok(esc) = ethercrab::Command::fprd(station, 0x0910u16) + .receive::(&maindevice).await + { + let loc = rt::now_ns(); + if let Some((e0, l0)) = dc_prev { + let dt_loc = loc.wrapping_sub(l0) as f64; + let dt_esc = esc.wrapping_sub(e0) as f64; + if dt_loc > 1_000_000.0 { + dc_drift_ppm = ((dt_loc - dt_esc) / dt_loc) * 1e6; + } + } + let err = loc.wrapping_sub(esc) as i64; + dc_err_ns = err; + // measurement only: do NOT write offsets (naive actuation wrapped clock) + if dc_prev.is_none() { + let _ = ethercrab::Command::fpwr(station, 0x0920u16) + .send(&maindevice, 0u64).await; // reset offset once to raw + } + if dc_first.is_none() { dc_first = Some((esc, loc)); } + dc_prev = Some((esc, loc)); + } + } + } let iter_t0 = Instant::now(); if let Some(p) = last_iter { let gap = iter_t0.duration_since(p).as_micros() as u64; @@ -2499,6 +2542,13 @@ async fn rfm_async_loop( unsafe { std::slice::from_raw_parts_mut(p, PAYLOAD_LEN) }; slot.copy_from_slice(&tx_latest[..PAYLOAD_LEN]); tx_inflight_seq = tx_latest_seq; + if false && dc_discipline && phase_est != 0 { + let now_w = rt::now_ns() as i64; + let mut target = now_w - phase_est + 1_000_000 + 500_000; + while target < now_w + 50_000 { target += 1_000_000; } + while (rt::now_ns() as i64) < target { core::hint::spin_loop(); } + wphase_st.push(((rt::now_ns() % 1_000_000) / 1000) as i64); + } let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR) .send(maindevice, &slot[..PAYLOAD_LEN]) .await @@ -2542,6 +2592,17 @@ async fn rfm_async_loop( if rx_buf[..8] != prev_rx8 { prev_rx8.copy_from_slice(&rx_buf[..8]); n_fresh += 1; + { + let p = (rt::now_ns() % 1_000_000) as i64; + let mut adj = p; + if phase_est != 0 { + let d = p - phase_est; + if d > 500_000 { adj = p - 1_000_000; } else if d < -500_000 { adj = p + 1_000_000; } + phase_est = phase_est.min(adj); + } else { phase_est = p; } + phase_est += 2; // 2ns creep per fresh event (~2us/s drift compensation) + phase_st.push((p / 1000) as i64); + } // 5. forward fresh CX data to RFM REQ immediately let t0 = Instant::now(); let n = SM_RX_PLD_LEN.min(MB_MAX_PAYLOAD); @@ -2645,6 +2706,12 @@ async fn rfm_async_loop( } println!("\n=== rfm-async done: {:.1}s ===", start.elapsed().as_secs_f64()); + print_stats(&phase_st); + print_stats(&wphase_st); + if let (Some((e0, l0)), Some((e1, l1))) = (dc_first, dc_prev) { + let ratio_ppm = ((e1.wrapping_sub(e0) as f64) / (l1.wrapping_sub(l0) as f64 - 1.0) - 1.0) * 1e6; + println!(" ESC-vs-local rate ratio: {:+.2} ppm (esc_free_running if nonzero)", ratio_ppm); + } println!( "fprd={} (err={}) fresh={} req={} rep={} fpwr={} (err={}) overflow={}", n_fprd, n_fprd_err, n_fresh, n_req, n_rep, n_fpwr, n_fpwr_err, n_ovf