feat(adr-110): SyncPacket::mesh_aligned_us_for_sequence (interpolation) + NodeState hook

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<u64>
    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 <ruv@ruv.net>
This commit is contained in:
ruv 2026-05-23 13:19:06 -04:00
parent df95360e52
commit 0c311a202b
2 changed files with 91 additions and 0 deletions

View File

@ -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 {

View File

@ -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<u64> {
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(),