diff --git a/firmware/esp32-csi-node/main/csi_collector.c b/firmware/esp32-csi-node/main/csi_collector.c index 0de753f0..bd31109d 100644 --- a/firmware/esp32-csi-node/main/csi_collector.c +++ b/firmware/esp32-csi-node/main/csi_collector.c @@ -214,6 +214,10 @@ static esp_timer_handle_t s_hop_timer = NULL; * [17] Noise floor (i8) * [18..19] Reserved * [20..] I/Q data (raw bytes from ESP-IDF callback) + * [20+iq_len .. 20+iq_len+3] ADR-106: sensor timestamp_us (u32 LE) + * from info->rx_ctrl.timestamp. Trailing + * 4 bytes — server parses opportunistically; + * old server tolerant of extra bytes. */ size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf_len) { @@ -225,7 +229,7 @@ size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf uint16_t iq_len = (uint16_t)info->len; uint16_t n_subcarriers = iq_len / (2 * n_antennas); - size_t frame_size = CSI_HEADER_SIZE + iq_len; + size_t frame_size = CSI_HEADER_SIZE + iq_len + 4 /* ADR-106 trailing timestamp_us */; if (frame_size > buf_len) { ESP_LOGW(TAG, "Buffer too small: need %u, have %u", (unsigned)frame_size, (unsigned)buf_len); return 0; @@ -278,6 +282,13 @@ size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf /* I/Q data */ memcpy(&buf[CSI_HEADER_SIZE], info->buf, iq_len); + /* ADR-106: trailing sensor µs timestamp from rx_ctrl.timestamp. + * This is monotonic µs since FW boot (per ESP-IDF docs) and lets + * the host align frames across nodes within ~µs once the boot + * offsets are learned. Old server ignores trailing bytes. */ + uint32_t ts_us = info->rx_ctrl.timestamp; + memcpy(&buf[CSI_HEADER_SIZE + iq_len], &ts_us, 4); + return frame_size; } diff --git a/v2/crates/wifi-densepose-sensing-server/src/main.rs b/v2/crates/wifi-densepose-sensing-server/src/main.rs index ae9c4529..075ad9a5 100644 --- a/v2/crates/wifi-densepose-sensing-server/src/main.rs +++ b/v2/crates/wifi-densepose-sensing-server/src/main.rs @@ -903,6 +903,10 @@ struct Esp32Frame { noise_floor: i8, amplitudes: Vec, phases: Vec, + /// ADR-106 trailing field — sensor µs timestamp from + /// `info->rx_ctrl.timestamp`. Monotonic µs since FW boot. + /// `None` for old FW that doesn't carry it. + sensor_timestamp_us: Option, } /// Sensing update broadcast to WebSocket clients @@ -1696,6 +1700,7 @@ fn parse_csi_lean(buf: &[u8]) -> Option { noise_floor: noise, amplitudes, phases, + sensor_timestamp_us: None, }) } @@ -1767,6 +1772,15 @@ fn parse_esp32_frame(buf: &[u8]) -> Option { noise_floor, amplitudes, phases, + // ADR-106: trailing 4-byte sensor µs timestamp from new FW. + // Old FW: buf has exactly `iq_start + iq_len` bytes ⇒ Option::None. + // New FW: 4 more bytes after I/Q ⇒ parse as u32 LE. + sensor_timestamp_us: if buf.len() >= expected_len + 4 { + Some(u32::from_le_bytes([ + buf[expected_len], buf[expected_len + 1], + buf[expected_len + 2], buf[expected_len + 3], + ])) + } else { None }, }) } @@ -2511,6 +2525,7 @@ async fn windows_wifi_task(state: SharedState, tick_ms: u64) { noise_floor: -90, amplitudes: multi_ap_frame.amplitudes.clone(), phases: multi_ap_frame.phases.clone(), + sensor_timestamp_us: None, }; // ── Step 4b: Update frame history and extract features ─────── @@ -2689,6 +2704,7 @@ async fn windows_wifi_fallback_tick(state: &SharedState, seq: u32) { noise_floor: -90, amplitudes: vec![signal_pct], phases: vec![0.0], + sensor_timestamp_us: None, }; let mut s = state.write().await; @@ -2846,6 +2862,7 @@ fn generate_simulated_frame(tick: u64) -> Esp32Frame { noise_floor: -90, amplitudes, phases, + sensor_timestamp_us: None, } } @@ -5247,18 +5264,19 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) { } ns.latest_noise_floor = frame.noise_floor; ns.latest_n_antennas = frame.n_antennas; - // ADR-106 follow-up: server-side receive timestamp - // in µs since UNIX epoch. Not as precise as - // sensor-side `info->rx_ctrl.timestamp` would be, - // but good enough for cross-node alignment within - // ~1 ms (Mac monotonic + LAN jitter). Sensor-side - // timestamp deferred to a future FW change that - // extends the 0xC511_0001 header — see ADR-106 - // Open Items. - ns.latest_timestamp_us = std::time::SystemTime::now() - .duration_since(std::time::UNIX_EPOCH) - .map(|d| d.as_micros() as u64) - .unwrap_or(0); + // ADR-106: prefer sensor's `rx_ctrl.timestamp` + // (monotonic µs since FW boot) when the new-FW + // trailing 4 bytes are present. Falls back to + // server SystemTime (UNIX µs) if old FW or peek + // failed. Two distinct reference frames; the + // serialized value is whichever was set. + ns.latest_timestamp_us = match frame.sensor_timestamp_us { + Some(ts) => ts as u64, // sensor monotonic µs + None => std::time::SystemTime::now() + .duration_since(std::time::UNIX_EPOCH) + .map(|d| d.as_micros() as u64) + .unwrap_or(0), + }; let sample_rate_hz = 1000.0 / 500.0_f64; let (features, mut classification, breathing_rate_hz, sub_variances, raw_motion) =