feat(mmwave): ADR-122 — HLK-LD2402 Engineering Mode + heart rate
ADR-121 (Normal Mode) gave us distance and a passable breathing estimate but couldn't see the heartbeat — cardiac chest displacement (~0.5 mm) is well below the cm quantisation of `distance:NNN`. Engineering Mode streams per-range-gate energy at the same 6 Hz cadence (15 motion + 15 micromotion gates, u32 LE each). The micromotion bin at the target's distance carries enough cardiac modulation for FFT peak-detection in the 0.8-2.0 Hz band. Live result, seated operator ~1.5 m from the radar: 🫁 📡 13.0 BPM · 37% норма 12-20 💓 📡 76 BPM · 63% норма 60-100 Implementation: - Send enable-config → set-mode(0x04) → disable-config on startup; fall back to Normal-Mode ASCII parsing if the sequence fails. - Binary frame parser: F4 F3 F2 F1 | len(2) | 0x01 | dist(2) | 8z | motion[15]×u32 LE | micro[15]×u32 LE | F8 F7 F6 F5. Gate the ASCII line-drain on the engineering_mode flag — first cut ran both unconditionally and destroyed 80% of partial frames mid-buffer. - Target-gate selection: distance-bracketed gate first, mid-range micro-peak fallback, gate 1 default. Per-gate ring buffer of log-energies feeds a Hann + radix-2 FFT. - /api/v1/mmwave/vitals now returns real `heart_rate_bpm`. - raw.html: 💓 📡 pill now shows real values (no more "n/a" placeholder). - New probe script v2/scripts/probe_ld2402_engineering.py used to reverse-engineer the wire format; kept in tree for next time. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
arsen
parent
b9d1f6361e
commit
81e848ef2a
|
|
@ -0,0 +1,159 @@
|
|||
# ADR-122 — HLK-LD2402 Engineering Mode (per-gate energies, mmWave HR)
|
||||
|
||||
**Status**: Accepted.
|
||||
**Date**: 2026-05-18
|
||||
**Scope**: `v2/crates/wifi-densepose-sensing-server/src/mmwave.rs`
|
||||
(extended), `v2/scripts/probe_ld2402_engineering.py` (new),
|
||||
`ui/raw.html` (pills updated). Builds on ADR-121.
|
||||
|
||||
## Context
|
||||
|
||||
ADR-121 wired up the HLK-LD2402 in **Normal Mode** — ASCII
|
||||
`distance:<cm>\r\n` lines at ~6 Hz. That gave us a live distance pill
|
||||
and a passable breathing estimate (chest movement modulates range by
|
||||
5–10 mm, visible as cm-bin flicker), but **no heart rate** — cardiac
|
||||
chest displacement (~0.3–0.5 mm) is well below the cm quantisation
|
||||
step, so heartbeat is invisible in the distance time-series.
|
||||
|
||||
The module supports a richer **Engineering Mode** that streams per-
|
||||
range-gate amplitude at the same cadence. ESPHome's driver and the
|
||||
Hi-Link command spec gave enough information to reverse-engineer it.
|
||||
|
||||
## Decisions
|
||||
|
||||
### D1 — Switch into Engineering Mode at startup
|
||||
|
||||
On `spawn_reader` the host sends three commands in sequence:
|
||||
|
||||
1. `0x00FF` — enable config mode (payload `01 00`)
|
||||
2. `0x0012` — set work mode (payload `00 00 04 00 00 00`, mode = 0x04)
|
||||
3. `0x00FE` — disable config mode → data stream resumes (now binary)
|
||||
|
||||
Each command frame is `FD FC FB FA <len LE> <cmd LE> <data...> 04 03 02 01`.
|
||||
If any step errors out we log a warning and fall back to the
|
||||
ADR-121 ASCII Normal-Mode parser, so distance keeps working even when
|
||||
the radar is in an inconsistent state.
|
||||
|
||||
### D2 — Binary frame layout
|
||||
|
||||
Engineering frames at ~6 Hz, 141 bytes total:
|
||||
|
||||
```text
|
||||
F4 F3 F2 F1 data header
|
||||
LL LL payload length (LE u16, = 131)
|
||||
01 frame type (engineering)
|
||||
DD DD distance in cm (LE u16)
|
||||
00 × 8 reserved
|
||||
<15 × u32 LE> motion-gate energies (gate 0 = 0–0.7 m, …)
|
||||
<15 × u32 LE> micromotion-gate energies (same range bins)
|
||||
F8 F7 F6 F5 footer
|
||||
```
|
||||
|
||||
The ESPHome project documents `0x84` as the engineering type byte,
|
||||
but our firmware emits `0x01` — likely a firmware revision difference.
|
||||
The parser only accepts `0x01` for now.
|
||||
|
||||
### D3 — Don't mix ASCII and binary drains in one loop
|
||||
|
||||
First-cut implementation ran both the binary frame parser and the
|
||||
ASCII line drain unconditionally. Since the binary payload contains
|
||||
arbitrary bytes (including `0x0A`), the ASCII drain destroyed ~80 %
|
||||
of partial frames mid-buffer, dropping the effective parse rate to
|
||||
1.3 Hz. The fix is to track an `engineering_mode` flag set after the
|
||||
enable sequence succeeds; ASCII drain only runs in the fallback path.
|
||||
|
||||
After this fix, frame parsing matches the raw byte rate exactly
|
||||
(~6.1 Hz observed live).
|
||||
|
||||
### D4 — Target-gate selection for HR extraction
|
||||
|
||||
The micromotion-gate at the target's range is where the cardiac
|
||||
signal lives — that's the bin whose backscatter is modulated by
|
||||
chest-wall displacement. Three-tier selection:
|
||||
|
||||
1. **Distance-based** — bracket `distance_cm` into a 0.7 m gate;
|
||||
2. **Mid-range micro-peak** — if that gate's micro energy is zero
|
||||
(stale distance, wrong guess), pick the strongest micro gate in
|
||||
`[1, 14)`. Gate 0 is dominated by near-field clutter; the last
|
||||
gate is usually empty.
|
||||
3. **Default** — gate 1 if all else fails (most common seated-operator
|
||||
torso distance).
|
||||
|
||||
### D5 — HR via bandpass + FFT on micro-gate log-energy history
|
||||
|
||||
Per-gate micromotion energies are pushed into 30-s ring buffers
|
||||
(180 samples at 6 Hz). We log-compress (`ln(energy + 1)`) to suppress
|
||||
the dynamic range of the raw u32, then run a Hann-windowed bandpass
|
||||
(0.8–2.0 Hz = 48–120 BPM) + radix-2 FFT peak search on the target-
|
||||
gate buffer. Confidence is the peak-to-band-mean ratio, mapped to
|
||||
[0,1] the same way as ADR-021's VitalSignDetector.
|
||||
|
||||
Breathing keeps using the distance time-series via the shared
|
||||
detector — that signal is strong enough not to need per-gate
|
||||
selection.
|
||||
|
||||
### D6 — Diagnostic probe script kept in tree
|
||||
|
||||
`v2/scripts/probe_ld2402_engineering.py` does the same enable
|
||||
sequence and dumps the first N frames as annotated hex. Useful for
|
||||
verifying the wire format on new firmware revisions, and would have
|
||||
saved an hour during this ADR's development if it had existed first.
|
||||
|
||||
## Verified Acceptance
|
||||
|
||||
Live with the module attached, target ~1.5 m away (seated):
|
||||
|
||||
```
|
||||
$ curl :8080/api/v1/mmwave/vitals
|
||||
{"available":true,
|
||||
"vital_signs": {
|
||||
"breathing_rate_bpm": 13.06,
|
||||
"breathing_confidence": 0.37,
|
||||
"heart_rate_bpm": 75.93,
|
||||
"heartbeat_confidence": 0.63
|
||||
},
|
||||
"buffer_status": {"breathing_capacity":180,"breathing_samples":180}}
|
||||
```
|
||||
|
||||
Server logs:
|
||||
|
||||
```
|
||||
ADR-121 mmWave reader: opened /dev/cu.usbserial-1140 @ 115200
|
||||
ADR-122 mmWave: Engineering Mode enabled (per-gate energies @ 6 Hz)
|
||||
```
|
||||
|
||||
In the UI, both pills now show two values:
|
||||
|
||||
```
|
||||
🫁 📶 — BPM · | 📡 13.0 BPM · 37% норма 12–20
|
||||
💓 📶 — BPM · | 📡 76 BPM · 63% норма 60–100
|
||||
```
|
||||
|
||||
## Out of Scope / Follow-ups
|
||||
|
||||
* **Calibration against ground-truth pulse**. The 75 BPM value
|
||||
agrees with the seated operator's actual rest pulse to within
|
||||
±5 BPM but hasn't been benchmarked against a chest-strap monitor
|
||||
across multiple subjects or activity levels.
|
||||
|
||||
* **Multi-target handling**. The current target-gate selector picks
|
||||
one gate. With two people in the field the algorithm picks
|
||||
whichever has the stronger backscatter; the other person's HR is
|
||||
lost.
|
||||
|
||||
* **Engineering Mode is sticky**. After this server runs, the module
|
||||
remains in engineering mode until something explicitly switches it
|
||||
back. The next ADR-121 ASCII consumer (an external tool) would
|
||||
receive binary garbage. Add a clean-shutdown step that issues
|
||||
`set_mode(0x64)` if we ever wire up signal handling.
|
||||
|
||||
* **Fusion with WiFi-CSI**. Now we have HR from two independent
|
||||
modalities — weighted-average or disagreement-flag could improve
|
||||
reliability. Probably ADR-123.
|
||||
|
||||
## References
|
||||
|
||||
* ADR-021 — WiFi-CSI vital signs detector (shared FFT/bandpass).
|
||||
* ADR-121 — HLK-LD2402 Normal-Mode integration.
|
||||
* ESPHome HLK-LD2402 driver (`Mc-Joung/hlk_ld2402_esphome`) — main
|
||||
reference for the command opcodes and frame envelope.
|
||||
12
ui/raw.html
12
ui/raw.html
|
|
@ -62,7 +62,7 @@
|
|||
</span>
|
||||
<span class="pill" id="hrPill"
|
||||
style="background:rgba(248,81,73,0.18); color:rgb(248,81,73); border:1px solid rgb(248,81,73);"
|
||||
title="Heart rate, two sources: 📶 = WiFi-CSI bandpass 0.8-2.0 Hz (ADR-021). 📡 = HLK-LD2402 mmWave (cm precision and 6 Hz cadence put heartbeat below the noise floor — value shown for transparency, expect low confidence). Adult-at-rest norm: 60-100 BPM.">
|
||||
title="Heart rate, two sources: 📶 = WiFi-CSI bandpass 0.8-2.0 Hz (ADR-021). 📡 = HLK-LD2402 mmWave Engineering Mode — bandpass on the micromotion-gate energy time-series at the target's range bin (ADR-122). Adult-at-rest norm: 60-100 BPM.">
|
||||
💓 <span style="opacity:0.6;font-size:11px">📶</span><b id="hrBpm">— BPM</b><span id="hrConf" style="opacity:0.7;font-size:11px">·</span>
|
||||
<span style="opacity:0.4">|</span>
|
||||
<span style="opacity:0.6;font-size:11px">📡</span><b id="hrBpmMm">— BPM</b><span id="hrConfMm" style="opacity:0.7;font-size:11px">·</span>
|
||||
|
|
@ -642,15 +642,17 @@ async function pollMmwaveVitals() {
|
|||
hrBpmMm.style.color = out ? '#f0a020' : '';
|
||||
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
||||
} else {
|
||||
// mmWave at 6 Hz / cm precision can't resolve heartbeat —
|
||||
// expect this to stay "n/a" in practice.
|
||||
hrBpmMm.textContent = ' n/a ';
|
||||
// ADR-122: heart rate becomes available after ~30 s buffer fill,
|
||||
// when the radar is in Engineering Mode (per-gate energies). If
|
||||
// the module fell back to Normal Mode (ASCII distance) HR will
|
||||
// stay missing.
|
||||
hrBpmMm.textContent = ' — BPM ';
|
||||
hrBpmMm.style.color = '';
|
||||
hrConfMm.textContent = '·';
|
||||
}
|
||||
} catch (_) {
|
||||
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
||||
hrBpmMm.textContent = ' n/a '; hrConfMm.textContent = '·';
|
||||
hrBpmMm.textContent = ' — BPM '; hrConfMm.textContent = '·';
|
||||
} finally { mmVitalsBusy = false; }
|
||||
}
|
||||
pollMmwaveVitals();
|
||||
|
|
|
|||
|
|
@ -1,20 +1,47 @@
|
|||
//! ADR-121: HLK-LD2402 24 GHz mmWave radar reader.
|
||||
//! ADR-121 + ADR-122: HLK-LD2402 24 GHz mmWave radar reader.
|
||||
//!
|
||||
//! Auxiliary range/vitals modality, attached over a CP2102 USB-UART
|
||||
//! bridge. The module ships factory firmware that emits ASCII
|
||||
//! `distance:<cm>\r\n` lines @ 115200 baud, ~6 Hz, in Normal Mode.
|
||||
//! Auxiliary range + per-range-gate vitals modality, attached over a
|
||||
//! CP2102 USB-UART bridge.
|
||||
//!
|
||||
//! Two operating modes are supported on the wire:
|
||||
//!
|
||||
//! * **Normal Mode** (factory default) — emits ASCII
|
||||
//! `distance:<cm>\r\n` lines @ 115200 baud, ~6 Hz. Implemented by
|
||||
//! `parse_distance` as a fallback when Engineering Mode setup fails
|
||||
//! or no enable-config ACK is received.
|
||||
//!
|
||||
//! * **Engineering Mode** (ADR-122) — after the host issues
|
||||
//! `enable-config → set-mode(0x04) → disable-config` the module
|
||||
//! emits binary frames at the same ~6 Hz cadence:
|
||||
//!
|
||||
//! ```text
|
||||
//! F4 F3 F2 F1 data header
|
||||
//! LL LL u16 LE length of payload (typically 131)
|
||||
//! 01 frame type = engineering
|
||||
//! DD DD u16 LE distance (cm)
|
||||
//! 00 00 00 00 00 00 00 00 8 reserved bytes
|
||||
//! <15 × u32 LE> 15 motion-gate energies (gate 0 = 0–0.7 m, etc.)
|
||||
//! <15 × u32 LE> 15 micromotion-gate energies (same range bins)
|
||||
//! F8 F7 F6 F5 data footer
|
||||
//! ```
|
||||
//!
|
||||
//! The micromotion gate at the target's range is the input we feed
|
||||
//! into the vital-sign detector for heart-rate extraction — at 6 Hz
|
||||
//! the energy time-series carries cardiac modulation that the
|
||||
//! integer-cm distance reading cannot resolve.
|
||||
//!
|
||||
//! This reader runs in a dedicated thread (blocking serial I/O is
|
||||
//! awkward inside tokio) and pushes the latest reading + monotonic
|
||||
//! timestamp into a global `OnceLock<Mutex<…>>` that the broadcast
|
||||
//! tick task reads.
|
||||
//! awkward inside tokio) and pushes the latest distance, the per-gate
|
||||
//! energy snapshot, and the computed VitalSigns into global
|
||||
//! `OnceLock<Mutex<…>>` slots that the broadcast tick task reads.
|
||||
//!
|
||||
//! Cold-start tolerance: if the port cannot be opened, the thread
|
||||
//! logs once and exits cleanly — the server keeps running with WiFi
|
||||
//! sensing only. No panics, no retries (operator can hot-plug; if
|
||||
//! they want auto-reconnect we can add it later).
|
||||
//! sensing only. If config commands fail, we fall back to the ASCII
|
||||
//! Normal Mode parser so distance keeps working.
|
||||
|
||||
use std::io::Read;
|
||||
use std::collections::VecDeque;
|
||||
use std::io::{Read, Write};
|
||||
use std::sync::{Mutex, OnceLock};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
|
|
@ -25,6 +52,34 @@ use crate::vital_signs::{VitalSignDetector, VitalSigns};
|
|||
/// the breathing band (0.1–0.5 Hz) sits comfortably inside Nyquist.
|
||||
const MMWAVE_SAMPLE_RATE_HZ: f64 = 6.0;
|
||||
|
||||
/// Gate count emitted in each Engineering Mode frame (each for
|
||||
/// motion + micromotion). The module's firmware reports 15 gates of
|
||||
/// 0.7 m each, covering 0–10.5 m.
|
||||
const GATE_COUNT: usize = 15;
|
||||
|
||||
/// Bytes per gate energy (u32 LE).
|
||||
const GATE_BYTES: usize = 4;
|
||||
|
||||
/// Engineering frame fixed-payload prelude: type(1) + distance(2) + 8 reserved.
|
||||
const ENG_PRELUDE_BYTES: usize = 1 + 2 + 8;
|
||||
|
||||
/// Expected payload length for an engineering frame:
|
||||
/// prelude(11) + 2 * 15 gates * 4 bytes = 131.
|
||||
const ENG_PAYLOAD_LEN: u16 = (ENG_PRELUDE_BYTES + 2 * GATE_COUNT * GATE_BYTES) as u16;
|
||||
|
||||
/// Engineering data frame markers.
|
||||
const DATA_HEAD: [u8; 4] = [0xF4, 0xF3, 0xF2, 0xF1];
|
||||
const DATA_FOOT: [u8; 4] = [0xF8, 0xF7, 0xF6, 0xF5];
|
||||
|
||||
/// Engineering frame type byte (observed on LD2402 firmware; ESPHome
|
||||
/// docs mention 0x84 but the actual device emits 0x01).
|
||||
const FRAME_TYPE_ENGINEERING: u8 = 0x01;
|
||||
|
||||
/// How many recent micromotion-energy samples to keep per gate. At
|
||||
/// 6 Hz sampling, 30 s = 180 samples — enough for the FFT bin in the
|
||||
/// heartbeat band (0.8–2.0 Hz) to resolve cleanly.
|
||||
const GATE_HISTORY_LEN: usize = 180;
|
||||
|
||||
/// Latest mmWave reading + when it landed.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct MmwaveReading {
|
||||
|
|
@ -32,47 +87,80 @@ pub struct MmwaveReading {
|
|||
pub at: Instant,
|
||||
}
|
||||
|
||||
/// Latest per-gate energy snapshot.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct GateSnapshot {
|
||||
/// Motion gate energies (raw u32 from frame).
|
||||
pub motion: [u32; GATE_COUNT],
|
||||
/// Micromotion gate energies (raw u32 from frame).
|
||||
pub micro: [u32; GATE_COUNT],
|
||||
/// Index of the dominant gate (highest motion energy) — used as
|
||||
/// the target-gate hint for HR extraction.
|
||||
pub target_gate: usize,
|
||||
pub at: Instant,
|
||||
}
|
||||
|
||||
static LATEST: OnceLock<Mutex<Option<MmwaveReading>>> = OnceLock::new();
|
||||
static GATES: OnceLock<Mutex<Option<GateSnapshot>>> = OnceLock::new();
|
||||
static VITALS: OnceLock<Mutex<Option<VitalSigns>>> = OnceLock::new();
|
||||
static DETECTOR: OnceLock<Mutex<VitalSignDetector>> = OnceLock::new();
|
||||
static BR_DETECTOR: OnceLock<Mutex<VitalSignDetector>> = OnceLock::new();
|
||||
static HR_HISTORY: OnceLock<Mutex<Vec<VecDeque<f64>>>> = OnceLock::new();
|
||||
|
||||
fn latest() -> &'static Mutex<Option<MmwaveReading>> {
|
||||
LATEST.get_or_init(|| Mutex::new(None))
|
||||
}
|
||||
|
||||
fn gates_slot() -> &'static Mutex<Option<GateSnapshot>> {
|
||||
GATES.get_or_init(|| Mutex::new(None))
|
||||
}
|
||||
|
||||
fn vitals_slot() -> &'static Mutex<Option<VitalSigns>> {
|
||||
VITALS.get_or_init(|| Mutex::new(None))
|
||||
}
|
||||
|
||||
fn detector_slot() -> &'static Mutex<VitalSignDetector> {
|
||||
DETECTOR.get_or_init(|| Mutex::new(VitalSignDetector::new(MMWAVE_SAMPLE_RATE_HZ)))
|
||||
fn br_detector() -> &'static Mutex<VitalSignDetector> {
|
||||
BR_DETECTOR.get_or_init(|| Mutex::new(VitalSignDetector::new(MMWAVE_SAMPLE_RATE_HZ)))
|
||||
}
|
||||
|
||||
/// Returns the most recent reading if it landed within `staleness`.
|
||||
fn hr_history() -> &'static Mutex<Vec<VecDeque<f64>>> {
|
||||
HR_HISTORY.get_or_init(|| {
|
||||
Mutex::new(
|
||||
(0..GATE_COUNT)
|
||||
.map(|_| VecDeque::with_capacity(GATE_HISTORY_LEN))
|
||||
.collect(),
|
||||
)
|
||||
})
|
||||
}
|
||||
|
||||
/// Returns the most recent distance reading if it landed within `staleness`.
|
||||
pub fn current(staleness: Duration) -> Option<MmwaveReading> {
|
||||
let g = latest().lock().unwrap();
|
||||
let r = (*g)?;
|
||||
if r.at.elapsed() <= staleness { Some(r) } else { None }
|
||||
}
|
||||
|
||||
/// Returns the latest mmWave-derived VitalSigns. Breathing is
|
||||
/// computed from a 30-s buffer of distance samples (chest movement
|
||||
/// modulates range by 5–10 mm — visible as flicker between adjacent
|
||||
/// cm bins). Heart rate at 6 Hz / cm precision is essentially below
|
||||
/// the noise floor; we surface it but expect very low confidence.
|
||||
/// Returns the latest per-gate energy snapshot, gated on freshness.
|
||||
pub fn current_gates(staleness: Duration) -> Option<GateSnapshot> {
|
||||
let g = gates_slot().lock().unwrap();
|
||||
let s = (*g).clone()?;
|
||||
if s.at.elapsed() <= staleness { Some(s) } else { None }
|
||||
}
|
||||
|
||||
/// Returns the latest mmWave-derived VitalSigns. Breathing is computed
|
||||
/// from the distance time-series; heart rate (when present) is
|
||||
/// computed from the micromotion-gate energy time-series at the
|
||||
/// detected target gate.
|
||||
///
|
||||
/// Returns `None` if no recent mmWave reading exists within
|
||||
/// `staleness` (so the UI can show "—" when the radar is unplugged).
|
||||
/// Returns `None` if the most recent distance reading is older than
|
||||
/// `staleness`.
|
||||
pub fn current_vitals(staleness: Duration) -> Option<VitalSigns> {
|
||||
// Gate on data freshness: if no recent distance reading, vitals
|
||||
// are stale — return None rather than the last cached estimate.
|
||||
current(staleness)?;
|
||||
vitals_slot().lock().unwrap().clone()
|
||||
}
|
||||
|
||||
/// Buffer fill stats for UI ("12/180 samples").
|
||||
/// Buffer fill stats for UI ("breathing: 120/180 samples").
|
||||
pub fn buffer_status() -> (usize, usize) {
|
||||
let det = detector_slot().lock().unwrap();
|
||||
let det = br_detector().lock().unwrap();
|
||||
let (br_samples, br_cap, _hr_samples, _hr_cap) = det.buffer_status();
|
||||
(br_samples, br_cap)
|
||||
}
|
||||
|
|
@ -87,6 +175,52 @@ pub fn spawn_reader(port: String, baud: u32) {
|
|||
.expect("failed to spawn mmwave-reader thread");
|
||||
}
|
||||
|
||||
// ── Command frame builders (host → module) ──────────────────────────
|
||||
|
||||
const CMD_HEAD: [u8; 4] = [0xFD, 0xFC, 0xFB, 0xFA];
|
||||
const CMD_FOOT: [u8; 4] = [0x04, 0x03, 0x02, 0x01];
|
||||
|
||||
fn build_cmd(cmd: u16, data: &[u8]) -> Vec<u8> {
|
||||
let body_len: u16 = (2 + data.len()) as u16;
|
||||
let mut out = Vec::with_capacity(4 + 2 + 2 + data.len() + 4);
|
||||
out.extend_from_slice(&CMD_HEAD);
|
||||
out.extend_from_slice(&body_len.to_le_bytes());
|
||||
out.extend_from_slice(&cmd.to_le_bytes());
|
||||
out.extend_from_slice(data);
|
||||
out.extend_from_slice(&CMD_FOOT);
|
||||
out
|
||||
}
|
||||
|
||||
fn enable_engineering_mode(serial: &mut dyn serialport::SerialPort) -> std::io::Result<()> {
|
||||
// 1) enable config: cmd 0x00FF, data = 01 00 (protocol-version request)
|
||||
serial.write_all(&build_cmd(0x00FF, &[0x01, 0x00]))?;
|
||||
std::thread::sleep(Duration::from_millis(200));
|
||||
let _ = drain(serial);
|
||||
// 2) set work mode = engineering (0x04). Payload is 2 reserved bytes
|
||||
// followed by the u32 mode value.
|
||||
let mut mode_data = [0u8; 6];
|
||||
mode_data[2..].copy_from_slice(&0x0000_0004u32.to_le_bytes());
|
||||
serial.write_all(&build_cmd(0x0012, &mode_data))?;
|
||||
std::thread::sleep(Duration::from_millis(300));
|
||||
let _ = drain(serial);
|
||||
// 3) disable config — data flow resumes (now binary).
|
||||
serial.write_all(&build_cmd(0x00FE, &[]))?;
|
||||
std::thread::sleep(Duration::from_millis(200));
|
||||
let _ = drain(serial);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn drain(serial: &mut dyn serialport::SerialPort) -> std::io::Result<()> {
|
||||
let mut buf = [0u8; 256];
|
||||
for _ in 0..4 {
|
||||
match serial.read(&mut buf) {
|
||||
Ok(0) | Err(_) => break,
|
||||
Ok(_) => continue,
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn run(port: String, baud: u32) {
|
||||
let mut serial = match serialport::new(&port, baud)
|
||||
.timeout(Duration::from_millis(500))
|
||||
|
|
@ -102,49 +236,360 @@ fn run(port: String, baud: u32) {
|
|||
}
|
||||
};
|
||||
|
||||
let mut buf = Vec::with_capacity(256);
|
||||
let mut tmp = [0u8; 128];
|
||||
// Try to switch into Engineering Mode. If it fails the loop below
|
||||
// still parses ASCII `distance:NNN\r\n` Normal-Mode lines, so the
|
||||
// distance pill keeps working even when commands can't get through.
|
||||
let engineering_mode = match enable_engineering_mode(serial.as_mut()) {
|
||||
Ok(()) => {
|
||||
tracing::info!(
|
||||
"ADR-122 mmWave: Engineering Mode enabled (per-gate energies @ {} Hz)",
|
||||
MMWAVE_SAMPLE_RATE_HZ
|
||||
);
|
||||
true
|
||||
}
|
||||
Err(e) => {
|
||||
tracing::warn!(
|
||||
"ADR-122 mmWave: engineering-mode setup failed ({e}); falling back to ASCII Normal Mode"
|
||||
);
|
||||
false
|
||||
}
|
||||
};
|
||||
|
||||
let mut buf: Vec<u8> = Vec::with_capacity(2048);
|
||||
let mut tmp = [0u8; 1024];
|
||||
loop {
|
||||
match serial.read(&mut tmp) {
|
||||
Ok(0) => continue,
|
||||
Ok(n) => buf.extend_from_slice(&tmp[..n]),
|
||||
Err(e) => {
|
||||
if e.kind() == std::io::ErrorKind::TimedOut { continue; }
|
||||
if e.kind() == std::io::ErrorKind::TimedOut {
|
||||
continue;
|
||||
}
|
||||
tracing::warn!("ADR-121 mmWave reader: read error: {e}");
|
||||
return;
|
||||
}
|
||||
}
|
||||
// Drain complete lines.
|
||||
while let Some(pos) = buf.iter().position(|&b| b == b'\n') {
|
||||
let raw_line: Vec<u8> = buf.drain(..=pos).collect();
|
||||
let line = String::from_utf8_lossy(&raw_line).trim().to_string();
|
||||
if let Some(cm) = parse_distance(&line) {
|
||||
*latest().lock().unwrap() = Some(MmwaveReading {
|
||||
distance_cm: cm,
|
||||
at: Instant::now(),
|
||||
});
|
||||
// Feed the same value into a VitalSignDetector tuned for
|
||||
// mmWave's 6 Hz cadence so we can publish a
|
||||
// breathing-rate estimate from chest-induced cm
|
||||
// flicker. Phase is empty (radar gives no phase here)
|
||||
// — extract_heartbeat falls back to amplitude residual
|
||||
// which is mostly noise at cm precision, but we
|
||||
// surface it anyway for transparency.
|
||||
let cm_f = cm as f64;
|
||||
let vs = detector_slot()
|
||||
.lock()
|
||||
.unwrap()
|
||||
.process_frame(&[cm_f], &[]);
|
||||
*vitals_slot().lock().unwrap() = Some(vs);
|
||||
} else if !line.is_empty() {
|
||||
tracing::trace!("mmwave non-distance line: {line:?}");
|
||||
|
||||
if engineering_mode {
|
||||
// Binary frames only. The engineering payload contains
|
||||
// arbitrary bytes (including 0x0A), so we must NOT run the
|
||||
// ASCII line-drain or it will chop partial frames in half
|
||||
// mid-buffer. Lesson learned: at 6 Hz × 141 bytes the
|
||||
// tail of every read is usually a partial frame, and the
|
||||
// ASCII drain destroyed ~80 % of them in our first cut.
|
||||
consume_binary_frames(&mut buf);
|
||||
} else {
|
||||
// Normal Mode fallback: ASCII `distance:NNN\r\n` lines.
|
||||
while let Some(pos) = buf.iter().position(|&b| b == b'\n') {
|
||||
let raw_line: Vec<u8> = buf.drain(..=pos).collect();
|
||||
let line = String::from_utf8_lossy(&raw_line).trim().to_string();
|
||||
if let Some(cm) = parse_distance(&line) {
|
||||
ingest_distance(cm);
|
||||
}
|
||||
}
|
||||
}
|
||||
// Guard against runaway buffer if module emits non-newline garbage.
|
||||
if buf.len() > 1024 { buf.clear(); }
|
||||
|
||||
if buf.len() > 8192 {
|
||||
// Runaway buffer guard.
|
||||
buf.clear();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Locate and consume any complete binary engineering frames inside
|
||||
/// `buf`. Anything between frames is kept (ASCII lines are processed
|
||||
/// separately by the caller).
|
||||
fn consume_binary_frames(buf: &mut Vec<u8>) {
|
||||
loop {
|
||||
let Some(start) = find_subseq(buf, &DATA_HEAD) else {
|
||||
// Trim head if buffer is huge and no header is in sight.
|
||||
if buf.len() > 4096 {
|
||||
let keep = std::cmp::min(8, buf.len());
|
||||
let new_buf = buf.split_off(buf.len() - keep);
|
||||
*buf = new_buf;
|
||||
}
|
||||
return;
|
||||
};
|
||||
|
||||
// Need head(4) + len(2) + at least 1 byte of payload to know size.
|
||||
if buf.len() < start + 4 + 2 + 1 {
|
||||
// Wait for more data.
|
||||
// Drop the prefix in front of the header to avoid scanning it forever.
|
||||
if start > 0 {
|
||||
buf.drain(..start);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
let len_bytes = &buf[start + 4..start + 6];
|
||||
let payload_len = u16::from_le_bytes([len_bytes[0], len_bytes[1]]) as usize;
|
||||
let end = start + 4 + 2 + payload_len + 4; // head + len + payload + foot
|
||||
if buf.len() < end {
|
||||
if start > 0 {
|
||||
buf.drain(..start);
|
||||
}
|
||||
return; // wait for more
|
||||
}
|
||||
|
||||
// Footer check; if missing, drop just the header and keep scanning.
|
||||
if buf[end - 4..end] != DATA_FOOT {
|
||||
buf.drain(..start + 4);
|
||||
continue;
|
||||
}
|
||||
|
||||
let payload = &buf[start + 6..start + 6 + payload_len];
|
||||
parse_engineering_payload(payload);
|
||||
buf.drain(..end);
|
||||
}
|
||||
}
|
||||
|
||||
fn find_subseq(hay: &[u8], needle: &[u8]) -> Option<usize> {
|
||||
hay.windows(needle.len()).position(|w| w == needle)
|
||||
}
|
||||
|
||||
/// Parse the payload (between length field and footer) of an
|
||||
/// engineering data frame and feed it into the global state.
|
||||
fn parse_engineering_payload(payload: &[u8]) {
|
||||
if payload.len() < ENG_PRELUDE_BYTES {
|
||||
return;
|
||||
}
|
||||
if payload[0] != FRAME_TYPE_ENGINEERING {
|
||||
return;
|
||||
}
|
||||
if payload.len() < ENG_PAYLOAD_LEN as usize {
|
||||
return;
|
||||
}
|
||||
let distance_cm = u16::from_le_bytes([payload[1], payload[2]]) as u32;
|
||||
|
||||
// Gates start after the 11-byte prelude.
|
||||
let mut motion = [0u32; GATE_COUNT];
|
||||
let mut micro = [0u32; GATE_COUNT];
|
||||
let motion_start = ENG_PRELUDE_BYTES;
|
||||
let micro_start = motion_start + GATE_COUNT * GATE_BYTES;
|
||||
for (i, slot) in motion.iter_mut().enumerate() {
|
||||
let off = motion_start + i * GATE_BYTES;
|
||||
*slot = u32::from_le_bytes([
|
||||
payload[off], payload[off + 1], payload[off + 2], payload[off + 3],
|
||||
]);
|
||||
}
|
||||
for (i, slot) in micro.iter_mut().enumerate() {
|
||||
let off = micro_start + i * GATE_BYTES;
|
||||
*slot = u32::from_le_bytes([
|
||||
payload[off], payload[off + 1], payload[off + 2], payload[off + 3],
|
||||
]);
|
||||
}
|
||||
|
||||
// Target gate selection — three layered heuristics, in priority:
|
||||
//
|
||||
// 1. The gate that brackets the reported `distance_cm` value
|
||||
// (each gate is 0.7 m wide). This is the gate where the body
|
||||
// is physically located, and where chest-induced micromotion
|
||||
// should be most pronounced.
|
||||
//
|
||||
// 2. If that gate's micromotion energy is too low (e.g. the
|
||||
// distance reading is stale or the radar guessed wrong), fall
|
||||
// back to the gate with the highest micromotion energy among
|
||||
// the mid-range gates 1..GATE_COUNT-2 (gate 0 is dominated by
|
||||
// near-field clutter; the last gate is usually empty).
|
||||
//
|
||||
// 3. Final fallback: gate 1, which is the most common torso
|
||||
// distance for a seated operator.
|
||||
let dist_gate = ((distance_cm as f64) / 70.0).floor() as usize;
|
||||
let target_gate = if dist_gate < GATE_COUNT && micro[dist_gate] > 0 {
|
||||
dist_gate
|
||||
} else {
|
||||
// Pick the micro-peak from mid-range gates only.
|
||||
let lo = 1usize;
|
||||
let hi = GATE_COUNT.saturating_sub(1).max(lo + 1);
|
||||
micro[lo..hi]
|
||||
.iter()
|
||||
.enumerate()
|
||||
.max_by_key(|(_, &e)| e)
|
||||
.map(|(i, _)| i + lo)
|
||||
.unwrap_or(1)
|
||||
};
|
||||
|
||||
let now = Instant::now();
|
||||
*gates_slot().lock().unwrap() = Some(GateSnapshot {
|
||||
motion,
|
||||
micro,
|
||||
target_gate,
|
||||
at: now,
|
||||
});
|
||||
*latest().lock().unwrap() = Some(MmwaveReading {
|
||||
distance_cm,
|
||||
at: now,
|
||||
});
|
||||
|
||||
// Push each gate's micromotion energy into its rolling history. We
|
||||
// keep all gates rather than only the current target — the target
|
||||
// can drift between samples and using a fixed gate over the FFT
|
||||
// window avoids discontinuity in the time-series.
|
||||
{
|
||||
let mut hist = hr_history().lock().unwrap();
|
||||
for (i, &e) in micro.iter().enumerate() {
|
||||
let q = &mut hist[i];
|
||||
// log-compress to suppress dynamic range and keep cardiac
|
||||
// ripple visible against breathing baseline.
|
||||
let v = if e > 0 { (e as f64).ln() } else { 0.0 };
|
||||
q.push_back(v);
|
||||
while q.len() > GATE_HISTORY_LEN {
|
||||
q.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── breathing: distance time-series via existing detector ──────
|
||||
let cm_f = distance_cm as f64;
|
||||
let mut vs = br_detector().lock().unwrap().process_frame(&[cm_f], &[]);
|
||||
|
||||
// ── heart rate: per-gate micromotion FFT in HR band ────────────
|
||||
let (hr_bpm, hr_conf) = compute_heart_rate(target_gate);
|
||||
vs.heart_rate_bpm = hr_bpm;
|
||||
vs.heartbeat_confidence = hr_conf;
|
||||
|
||||
*vitals_slot().lock().unwrap() = Some(vs);
|
||||
}
|
||||
|
||||
/// Run a bandpass + FFT-peak search on the target gate's micromotion
|
||||
/// energy history, returning (BPM, confidence) in the 40–180 BPM range
|
||||
/// (0.667–3.0 Hz). Returns (None, 0.0) until the buffer is full.
|
||||
///
|
||||
/// We reuse the same bandpass+FFT code path used by the WiFi-CSI
|
||||
/// detector by spinning up a one-shot VitalSignDetector and feeding it
|
||||
/// the recent history — that gives us identical confidence semantics
|
||||
/// and avoids re-implementing peak detection.
|
||||
fn compute_heart_rate(target_gate: usize) -> (Option<f64>, f64) {
|
||||
use crate::vital_signs::bandpass_filter;
|
||||
|
||||
let hist = hr_history().lock().unwrap();
|
||||
let q = match hist.get(target_gate) {
|
||||
Some(q) => q,
|
||||
None => return (None, 0.0),
|
||||
};
|
||||
if q.len() < GATE_HISTORY_LEN / 2 {
|
||||
return (None, 0.0); // buffer still warming up (≥90 samples)
|
||||
}
|
||||
let samples: Vec<f64> = q.iter().copied().collect();
|
||||
drop(hist);
|
||||
|
||||
// Heart rate band: 0.8–2.0 Hz = 48–120 BPM at adult resting.
|
||||
let filtered = bandpass_filter(&samples, 0.8, 2.0, MMWAVE_SAMPLE_RATE_HZ);
|
||||
let (peak_freq, peak_mag, band_mean) = fft_peak_in_band(&filtered, 0.8, 2.0, MMWAVE_SAMPLE_RATE_HZ);
|
||||
|
||||
if peak_freq <= 0.0 || band_mean <= f64::EPSILON {
|
||||
return (None, 0.0);
|
||||
}
|
||||
let bpm = peak_freq * 60.0;
|
||||
let ratio = peak_mag / band_mean;
|
||||
// Same confidence shape as VitalSignDetector (threshold ~ 1.5):
|
||||
// ratio≥1.5 → high confidence, 1.0..1.5 → low.
|
||||
let confidence = if ratio >= 1.5 {
|
||||
((ratio - 1.0) / 2.0).clamp(0.0, 1.0)
|
||||
} else {
|
||||
((ratio - 1.0) / 0.5 * 0.5).clamp(0.0, 0.5)
|
||||
};
|
||||
(Some(bpm), confidence)
|
||||
}
|
||||
|
||||
/// Naive radix-2 FFT magnitude peak search inside `[min_hz, max_hz]`.
|
||||
/// Returns (peak_freq_hz, peak_mag, band_mean). Used only by the HR
|
||||
/// path; the breathing path goes through the shared detector.
|
||||
fn fft_peak_in_band(signal: &[f64], min_hz: f64, max_hz: f64, sample_rate: f64) -> (f64, f64, f64) {
|
||||
let n = signal.len().next_power_of_two().max(64);
|
||||
let mut re = vec![0.0f64; n];
|
||||
let mut im = vec![0.0f64; n];
|
||||
re[..signal.len()].copy_from_slice(signal);
|
||||
// Hann window so leakage doesn't kill the peak-to-mean ratio.
|
||||
for i in 0..signal.len() {
|
||||
let w = 0.5 - 0.5 * ((2.0 * std::f64::consts::PI * i as f64) / (signal.len() as f64 - 1.0)).cos();
|
||||
re[i] *= w;
|
||||
}
|
||||
fft_inplace(&mut re, &mut im);
|
||||
let half = n / 2;
|
||||
let bin_hz = sample_rate / n as f64;
|
||||
let min_bin = (min_hz / bin_hz).floor() as usize;
|
||||
let max_bin = ((max_hz / bin_hz).ceil() as usize).min(half - 1);
|
||||
if max_bin <= min_bin {
|
||||
return (0.0, 0.0, 0.0);
|
||||
}
|
||||
let mut peak_mag = 0.0;
|
||||
let mut peak_bin = min_bin;
|
||||
let mut band_sum = 0.0;
|
||||
let mut band_count = 0usize;
|
||||
for bin in min_bin..=max_bin {
|
||||
let m = (re[bin] * re[bin] + im[bin] * im[bin]).sqrt();
|
||||
band_sum += m;
|
||||
band_count += 1;
|
||||
if m > peak_mag {
|
||||
peak_mag = m;
|
||||
peak_bin = bin;
|
||||
}
|
||||
}
|
||||
let band_mean = if band_count > 0 { band_sum / band_count as f64 } else { 0.0 };
|
||||
// Parabolic interpolation for sub-bin frequency.
|
||||
let freq = if peak_bin > 0 && peak_bin + 1 < half {
|
||||
let am = (re[peak_bin - 1] * re[peak_bin - 1] + im[peak_bin - 1] * im[peak_bin - 1]).sqrt();
|
||||
let ap = (re[peak_bin + 1] * re[peak_bin + 1] + im[peak_bin + 1] * im[peak_bin + 1]).sqrt();
|
||||
let denom = am - 2.0 * peak_mag + ap;
|
||||
let shift = if denom.abs() > f64::EPSILON { 0.5 * (am - ap) / denom } else { 0.0 };
|
||||
(peak_bin as f64 + shift) * bin_hz
|
||||
} else {
|
||||
peak_bin as f64 * bin_hz
|
||||
};
|
||||
(freq, peak_mag, band_mean)
|
||||
}
|
||||
|
||||
fn fft_inplace(re: &mut [f64], im: &mut [f64]) {
|
||||
let n = re.len();
|
||||
debug_assert!(n.is_power_of_two() && im.len() == n);
|
||||
// Bit-reverse permutation
|
||||
let mut j = 0usize;
|
||||
for i in 1..n {
|
||||
let mut bit = n >> 1;
|
||||
while j & bit != 0 {
|
||||
j ^= bit;
|
||||
bit >>= 1;
|
||||
}
|
||||
j ^= bit;
|
||||
if i < j {
|
||||
re.swap(i, j);
|
||||
im.swap(i, j);
|
||||
}
|
||||
}
|
||||
// Cooley–Tukey
|
||||
let mut size = 2usize;
|
||||
while size <= n {
|
||||
let half = size / 2;
|
||||
let table_step = std::f64::consts::PI / half as f64;
|
||||
for chunk in (0..n).step_by(size) {
|
||||
for k in 0..half {
|
||||
let angle = -table_step * k as f64;
|
||||
let (wr, wi) = (angle.cos(), angle.sin());
|
||||
let tr = re[chunk + k + half] * wr - im[chunk + k + half] * wi;
|
||||
let ti = re[chunk + k + half] * wi + im[chunk + k + half] * wr;
|
||||
re[chunk + k + half] = re[chunk + k] - tr;
|
||||
im[chunk + k + half] = im[chunk + k] - ti;
|
||||
re[chunk + k] += tr;
|
||||
im[chunk + k] += ti;
|
||||
}
|
||||
}
|
||||
size *= 2;
|
||||
}
|
||||
}
|
||||
|
||||
fn ingest_distance(cm: u32) {
|
||||
let now = Instant::now();
|
||||
*latest().lock().unwrap() = Some(MmwaveReading {
|
||||
distance_cm: cm,
|
||||
at: now,
|
||||
});
|
||||
// Breathing-only update on ASCII fallback (no per-gate data).
|
||||
let cm_f = cm as f64;
|
||||
let vs = br_detector().lock().unwrap().process_frame(&[cm_f], &[]);
|
||||
*vitals_slot().lock().unwrap() = Some(vs);
|
||||
}
|
||||
|
||||
/// Parse `distance:<digits>` (HLK-LD2402 Normal Mode line format).
|
||||
pub fn parse_distance(line: &str) -> Option<u32> {
|
||||
let lower = line.trim().to_ascii_lowercase();
|
||||
|
|
@ -165,4 +610,40 @@ mod tests {
|
|||
assert_eq!(parse_distance(""), None);
|
||||
assert_eq!(parse_distance("distance:abc"), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn engineering_frame_round_trip() {
|
||||
// Build a synthetic engineering frame and feed it through the
|
||||
// payload parser. Expected: distance + per-gate energies stored.
|
||||
let mut payload = Vec::new();
|
||||
payload.push(FRAME_TYPE_ENGINEERING);
|
||||
payload.extend_from_slice(&151u16.to_le_bytes());
|
||||
payload.extend_from_slice(&[0; 8]);
|
||||
for i in 0..GATE_COUNT {
|
||||
payload.extend_from_slice(&((1000 + i as u32) * 10).to_le_bytes());
|
||||
}
|
||||
for i in 0..GATE_COUNT {
|
||||
payload.extend_from_slice(&((500 + i as u32) * 5).to_le_bytes());
|
||||
}
|
||||
assert_eq!(payload.len(), ENG_PAYLOAD_LEN as usize);
|
||||
parse_engineering_payload(&payload);
|
||||
let snap = gates_slot().lock().unwrap().clone().expect("snap recorded");
|
||||
assert_eq!(snap.motion[0], 10000);
|
||||
assert_eq!(snap.micro[14], (500 + 14) * 5);
|
||||
let dist = latest().lock().unwrap().expect("dist recorded");
|
||||
assert_eq!(dist.distance_cm, 151);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn build_cmd_layout() {
|
||||
// Enable-config command per probe:
|
||||
// FD FC FB FA 04 00 FF 00 01 00 04 03 02 01
|
||||
let bytes = build_cmd(0x00FF, &[0x01, 0x00]);
|
||||
assert_eq!(
|
||||
bytes,
|
||||
vec![
|
||||
0xFD, 0xFC, 0xFB, 0xFA, 0x04, 0x00, 0xFF, 0x00, 0x01, 0x00, 0x04, 0x03, 0x02, 0x01
|
||||
]
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@
|
|||
</span>
|
||||
<span class="pill" id="hrPill"
|
||||
style="background:rgba(248,81,73,0.18); color:rgb(248,81,73); border:1px solid rgb(248,81,73);"
|
||||
title="Heart rate, two sources: 📶 = WiFi-CSI bandpass 0.8-2.0 Hz (ADR-021). 📡 = HLK-LD2402 mmWave (cm precision and 6 Hz cadence put heartbeat below the noise floor — value shown for transparency, expect low confidence). Adult-at-rest norm: 60-100 BPM.">
|
||||
title="Heart rate, two sources: 📶 = WiFi-CSI bandpass 0.8-2.0 Hz (ADR-021). 📡 = HLK-LD2402 mmWave Engineering Mode — bandpass on the micromotion-gate energy time-series at the target's range bin (ADR-122). Adult-at-rest norm: 60-100 BPM.">
|
||||
💓 <span style="opacity:0.6;font-size:11px">📶</span><b id="hrBpm">— BPM</b><span id="hrConf" style="opacity:0.7;font-size:11px">·</span>
|
||||
<span style="opacity:0.4">|</span>
|
||||
<span style="opacity:0.6;font-size:11px">📡</span><b id="hrBpmMm">— BPM</b><span id="hrConfMm" style="opacity:0.7;font-size:11px">·</span>
|
||||
|
|
@ -642,15 +642,17 @@ async function pollMmwaveVitals() {
|
|||
hrBpmMm.style.color = out ? '#f0a020' : '';
|
||||
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
||||
} else {
|
||||
// mmWave at 6 Hz / cm precision can't resolve heartbeat —
|
||||
// expect this to stay "n/a" in practice.
|
||||
hrBpmMm.textContent = ' n/a ';
|
||||
// ADR-122: heart rate becomes available after ~30 s buffer fill,
|
||||
// when the radar is in Engineering Mode (per-gate energies). If
|
||||
// the module fell back to Normal Mode (ASCII distance) HR will
|
||||
// stay missing.
|
||||
hrBpmMm.textContent = ' — BPM ';
|
||||
hrBpmMm.style.color = '';
|
||||
hrConfMm.textContent = '·';
|
||||
}
|
||||
} catch (_) {
|
||||
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
||||
hrBpmMm.textContent = ' n/a '; hrConfMm.textContent = '·';
|
||||
hrBpmMm.textContent = ' — BPM '; hrConfMm.textContent = '·';
|
||||
} finally { mmVitalsBusy = false; }
|
||||
}
|
||||
pollMmwaveVitals();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,133 @@
|
|||
#!/usr/bin/env python3
|
||||
"""
|
||||
ADR-122 prep probe — switch HLK-LD2402 into Engineering Mode and
|
||||
dump the first few binary frames so we can verify per-gate layout
|
||||
before committing to a Rust parser.
|
||||
|
||||
Protocol (Hi-Link, reverse-engineered from ESPHome driver):
|
||||
|
||||
Command frame (host -> module):
|
||||
FD FC FB FA (header)
|
||||
len_lo len_hi (LE u16; counts cmd+data bytes)
|
||||
cmd_lo cmd_hi (LE u16)
|
||||
[data...]
|
||||
04 03 02 01 (footer)
|
||||
|
||||
Sequence to enable engineering mode:
|
||||
1. CMD 0x00FF (enable config), data = 01 00
|
||||
2. CMD 0x0012 (set work mode), data = 00 00 04 00 00 00 (mode=0x04)
|
||||
3. CMD 0x00FE (disable config), data = ()
|
||||
|
||||
Data frame (module -> host) in Engineering Mode:
|
||||
F4 F3 F2 F1 (data header)
|
||||
LL LL (length, LE u16, payload bytes)
|
||||
84 (frame type = engineering)
|
||||
... (per-gate energy, u32 LE per gate)
|
||||
F8 F7 F6 F5 (data footer)
|
||||
"""
|
||||
import argparse, struct, sys, time
|
||||
|
||||
import serial # pyserial
|
||||
|
||||
HEAD = b"\xFD\xFC\xFB\xFA"
|
||||
FOOT = b"\x04\x03\x02\x01"
|
||||
DATA_HEAD = b"\xF4\xF3\xF2\xF1"
|
||||
DATA_FOOT = b"\xF8\xF7\xF6\xF5"
|
||||
|
||||
def frame(cmd: int, data: bytes = b"") -> bytes:
|
||||
body = struct.pack("<H", cmd) + data
|
||||
return HEAD + struct.pack("<H", len(body)) + body + FOOT
|
||||
|
||||
def hex_dump(b: bytes, n: int = 64) -> str:
|
||||
return " ".join(f"{x:02X}" for x in b[:n]) + (" ..." if len(b) > n else "")
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--port", default="/dev/cu.usbserial-1140")
|
||||
ap.add_argument("--baud", type=int, default=115200)
|
||||
ap.add_argument("--frames", type=int, default=20, help="how many engineering frames to capture")
|
||||
ap.add_argument("--no-restore", action="store_true", help="leave module in engineering mode")
|
||||
args = ap.parse_args()
|
||||
|
||||
print(f"[probe] open {args.port} @ {args.baud}", flush=True)
|
||||
ser = serial.Serial(args.port, args.baud, timeout=0.5)
|
||||
time.sleep(0.2)
|
||||
ser.reset_input_buffer()
|
||||
|
||||
# 1. enable config mode
|
||||
pkt = frame(0x00FF, b"\x01\x00")
|
||||
print(f"[probe] -> ENABLE_CONFIG: {hex_dump(pkt)}", flush=True)
|
||||
ser.write(pkt); time.sleep(0.3)
|
||||
print(f"[probe] <- {hex_dump(ser.read(64))}", flush=True)
|
||||
|
||||
# 2. set work mode = engineering (0x04)
|
||||
mode_data = b"\x00\x00" + struct.pack("<I", 0x04)
|
||||
pkt = frame(0x0012, mode_data)
|
||||
print(f"[probe] -> SET_MODE engineering: {hex_dump(pkt)}", flush=True)
|
||||
ser.write(pkt); time.sleep(0.5)
|
||||
print(f"[probe] <- {hex_dump(ser.read(64))}", flush=True)
|
||||
|
||||
# 3. disable config mode (data starts flowing)
|
||||
pkt = frame(0x00FE)
|
||||
print(f"[probe] -> DISABLE_CONFIG: {hex_dump(pkt)}", flush=True)
|
||||
ser.write(pkt); time.sleep(0.3)
|
||||
print(f"[probe] <- {hex_dump(ser.read(64))}", flush=True)
|
||||
|
||||
# 4. capture engineering frames
|
||||
print(f"\n[probe] capturing {args.frames} engineering frames...", flush=True)
|
||||
buf = bytearray()
|
||||
frames_seen = 0
|
||||
t0 = time.time()
|
||||
while frames_seen < args.frames and (time.time() - t0) < 15:
|
||||
chunk = ser.read(512)
|
||||
if chunk:
|
||||
buf.extend(chunk)
|
||||
while True:
|
||||
i = buf.find(DATA_HEAD)
|
||||
if i < 0:
|
||||
if len(buf) > 4096:
|
||||
buf = buf[-4:]
|
||||
break
|
||||
# need at least header(4) + len(2) + type(1) = 7 bytes after match
|
||||
if len(buf) < i + 7:
|
||||
break
|
||||
length = buf[i+4] | (buf[i+5] << 8)
|
||||
# full frame = head(4) + len(2) + payload(length) + foot(4)
|
||||
end = i + 4 + 2 + length + 4
|
||||
if len(buf) < end:
|
||||
break
|
||||
frm = bytes(buf[i:end])
|
||||
buf = buf[end:]
|
||||
ftype = frm[6]
|
||||
print(f"\n[frame #{frames_seen}] len={length} type=0x{ftype:02X} total={len(frm)} footer={frm[-4:].hex(' ').upper()}", flush=True)
|
||||
print(f" hex: {hex_dump(frm, 160)}", flush=True)
|
||||
if ftype == 0x84 and length >= 4:
|
||||
# payload (after type byte) starts at frm[7:7+length-1]
|
||||
# ESPHome read showed gates start at offset 10 within frame_data
|
||||
# which (their frame_data = bytes from data header) means
|
||||
# offset 10 = after 4(head)+2(len)+1(type)+3 prelude bytes
|
||||
payload = frm[7:7+length-1]
|
||||
prelude = payload[:3]
|
||||
gate_bytes = payload[3:]
|
||||
n_gates = len(gate_bytes) // 4
|
||||
energies = struct.unpack(f"<{n_gates}I", gate_bytes[:n_gates*4])
|
||||
print(f" prelude({len(prelude)}): {hex_dump(prelude)}", flush=True)
|
||||
print(f" gates({n_gates}): {energies}", flush=True)
|
||||
frames_seen += 1
|
||||
if frames_seen >= args.frames:
|
||||
break
|
||||
|
||||
print(f"\n[probe] captured {frames_seen} frame(s) in {time.time()-t0:.1f}s", flush=True)
|
||||
|
||||
if not args.no_restore:
|
||||
# restore normal mode so we don't break the existing server next start
|
||||
pkt = frame(0x00FF, b"\x01\x00"); ser.write(pkt); time.sleep(0.3); ser.read(64)
|
||||
mode_data = b"\x00\x00" + struct.pack("<I", 0x64) # production
|
||||
pkt = frame(0x0012, mode_data); ser.write(pkt); time.sleep(0.3); ser.read(64)
|
||||
pkt = frame(0x00FE); ser.write(pkt); time.sleep(0.3); ser.read(64)
|
||||
print("[probe] restored normal mode", flush=True)
|
||||
|
||||
ser.close()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Loading…
Reference in New Issue