From 0c311a202b34b484ac1cd1011b707a26d299ee34 Mon Sep 17 00:00:00 2001 From: ruv Date: Sat, 23 May 2026 13:19:06 -0400 Subject: [PATCH] feat(adr-110): SyncPacket::mesh_aligned_us_for_sequence (interpolation) + NodeState hook MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Iter 17 — closes the per-frame mesh-time loop for ADR-018 CSI frames that carry no per-frame local_us field (the v1 wire format reserves no slot — see WITNESS-LOG-110 §A0.11). Math: pair the frame's sequence number against the sync packet's sequence high-water + an assumed CSI frame rate. Δframes × 1/fps estimates the node-local delta from the sync, then apply_to_local recovers the mesh epoch. SyncPacket::mesh_aligned_us_for_sequence(frame_seq: u32, fps_hz: f64) -> u64 3 new unit tests (13 total in sync_packet::tests, all green): * mesh_aligned_for_sequence_identity_at_sync_point — at sync.sequence returns sync.epoch_us exactly * mesh_aligned_for_sequence_extrapolates_forward — 20 frames @ 20 fps extrapolates by exactly 1 s * mesh_aligned_for_sequence_handles_seq_wraparound — u32 sequence wrap doesn't jump backward by 2^32 (wrapping_sub guards it) NodeState hook: NodeState::mesh_aligned_us_for_csi_frame(frame_sequence: u32) -> Option Wraps the SyncPacket method, defaults fps_hz=20.0 (matches the firmware's CSI_MIN_SEND_INTERVAL_US-implied ceiling), enforces the same 9 s staleness gate as mesh_aligned_us. cargo check -p wifi-densepose-sensing-server --no-default-features → green. cargo test -p wifi-densepose-hardware sync_packet → 13/13, 122 filtered. Downstream ADR-029/030 multistatic fusion code can now do: if frame.adr018_flags.ieee802154_sync_valid { if let Some(mesh_us) = ns.mesh_aligned_us_for_csi_frame(frame.sequence) { // pair this frame with frames from sibling nodes by mesh_us } } Co-Authored-By: claude-flow --- .../src/sync_packet.rs | 74 +++++++++++++++++++ .../wifi-densepose-sensing-server/src/main.rs | 17 +++++ 2 files changed, 91 insertions(+) diff --git a/v2/crates/wifi-densepose-hardware/src/sync_packet.rs b/v2/crates/wifi-densepose-hardware/src/sync_packet.rs index 5b642a0c..fa7429d1 100644 --- a/v2/crates/wifi-densepose-hardware/src/sync_packet.rs +++ b/v2/crates/wifi-densepose-hardware/src/sync_packet.rs @@ -149,6 +149,39 @@ impl SyncPacket { (local_at_frame_us as i64).wrapping_add(offset) as u64 } + /// Recover the mesh-aligned timestamp for an in-flight CSI frame + /// **using its ADR-018 sequence number** as the timeline anchor. + /// + /// CSI frames carry no per-frame `local_us` field (ADR-018 v1 wire + /// format reserves no slot for it — see WITNESS-LOG-110 §A0.11), + /// but they do carry a 32-bit sequence number. The firmware emits + /// a sync packet alongside CSI frames, stamping the sequence + /// high-water observed at emit time into [`SyncPacket::sequence`]. + /// + /// Given a frame's sequence and the node's observed CSI frame rate, + /// estimate the node-local time at the frame and apply the mesh + /// offset: + /// + /// ```text + /// Δframes = frame_seq - sync.sequence (wrapping) + /// Δus = Δframes × 1_000_000 / fps_hz (node-local) + /// local_at = sync.local_us + Δus + /// mesh = local_at + (sync.epoch_us - sync.local_us) + /// ``` + /// + /// `fps_hz` must be > 0; pass the firmware's `CSI_MIN_SEND_INTERVAL_US` + /// inverse (≈ 20 fps) or a measured rate from the broadcast-tick task. + /// The estimate is exact when the frame rate is stable (a node holding + /// 20 fps within ±1 frame for the sync→frame interval gives + /// |error| < 1/fps_hz ≈ 50 ms × the per-frame jitter ratio). + pub fn mesh_aligned_us_for_sequence(&self, frame_seq: u32, fps_hz: f64) -> u64 { + debug_assert!(fps_hz > 0.0, "fps_hz must be positive"); + let dframes = (frame_seq.wrapping_sub(self.sequence)) as i64; + let dus = (dframes as f64 * 1_000_000.0 / fps_hz) as i64; + let local_at = (self.local_us as i64).wrapping_add(dus) as u64; + self.apply_to_local(local_at) + } + /// Serialize back to wire bytes (32 bytes, little-endian). pub fn to_bytes(&self) -> [u8; SYNC_PACKET_SIZE] { let mut out = [0u8; SYNC_PACKET_SIZE]; @@ -309,6 +342,47 @@ mod tests { mesh as i64 - frame_local as i64); } + /// At the sync packet's own sequence number, the interpolated mesh + /// time must equal `epoch_us` exactly. + #[test] + fn mesh_aligned_for_sequence_identity_at_sync_point() { + let pkt = SyncPacket { + node_id: 9, proto_ver: 1, + flags: SyncPacketFlags { is_leader: false, is_valid: true, smoothed_used: true }, + local_us: 28_798_450, epoch_us: 27_634_885, sequence: 20, + }; + assert_eq!(pkt.mesh_aligned_us_for_sequence(20, 20.0), pkt.epoch_us); + } + + /// 20 frames after the sync packet at 20 Hz → mesh time advances by 1 s, + /// preserving the leader/follower clock offset. + #[test] + fn mesh_aligned_for_sequence_extrapolates_forward() { + let pkt = SyncPacket { + node_id: 9, proto_ver: 1, + flags: SyncPacketFlags { is_leader: false, is_valid: true, smoothed_used: true }, + local_us: 28_798_450, epoch_us: 27_634_885, sequence: 20, + }; + // 20 frames at 20 fps = 1 000 000 µs + let mesh = pkt.mesh_aligned_us_for_sequence(40, 20.0); + assert_eq!(mesh, pkt.epoch_us + 1_000_000); + } + + /// Sequence wraparound (u32 overflow) must extrapolate forward by one + /// frame, not jump backward by 2^32. The wrapping_sub semantics in + /// the implementation guard this. + #[test] + fn mesh_aligned_for_sequence_handles_seq_wraparound() { + let pkt = SyncPacket { + node_id: 9, proto_ver: 1, + flags: SyncPacketFlags { is_leader: false, is_valid: true, smoothed_used: true }, + local_us: 10_000, epoch_us: 10_000, sequence: u32::MAX, + }; + // Next sequence after u32::MAX is 0 (wrap). Δframes = 1, not -2^32. + let mesh = pkt.mesh_aligned_us_for_sequence(0, 20.0); + assert_eq!(mesh, pkt.epoch_us + 50_000); // 1 frame at 20 fps = 50 ms + } + #[test] fn wire_size_constant_is_correct() { let pkt = SyncPacket { diff --git a/v2/crates/wifi-densepose-sensing-server/src/main.rs b/v2/crates/wifi-densepose-sensing-server/src/main.rs index 355dd4d3..6fb07fd5 100644 --- a/v2/crates/wifi-densepose-sensing-server/src/main.rs +++ b/v2/crates/wifi-densepose-sensing-server/src/main.rs @@ -436,6 +436,23 @@ impl NodeState { Some(sync.apply_to_local(local_at_frame_us)) } + /// ADR-110 §A0.12 sequence-based mesh-time recovery for an in-flight + /// ADR-018 CSI frame. The frame carries no `local_us` (the wire + /// format has no slot), but it carries a sequence number that the + /// sync packet's `sequence` high-water can be paired against. Uses + /// 20 Hz as the default CSI rate (the firmware's + /// `CSI_MIN_SEND_INTERVAL_US`-implied ceiling). Returns `None` if + /// no fresh sync has been observed for this node. + pub(crate) fn mesh_aligned_us_for_csi_frame(&self, frame_sequence: u32) -> Option { + let sync = self.latest_sync.as_ref()?; + let seen_at = self.latest_sync_at?; + if seen_at.elapsed() > std::time::Duration::from_secs(9) { + return None; + } + const CSI_FPS_HZ: f64 = 20.0; + Some(sync.mesh_aligned_us_for_sequence(frame_sequence, CSI_FPS_HZ)) + } + pub(crate) fn new() -> Self { Self { frame_history: VecDeque::new(),