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:
parent
df95360e52
commit
0c311a202b
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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(),
|
||||
|
|
|
|||
Loading…
Reference in New Issue