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>
|
||||||
<span class="pill" id="hrPill"
|
<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);"
|
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.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.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>
|
<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' : '';
|
hrBpmMm.style.color = out ? '#f0a020' : '';
|
||||||
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
||||||
} else {
|
} else {
|
||||||
// mmWave at 6 Hz / cm precision can't resolve heartbeat —
|
// ADR-122: heart rate becomes available after ~30 s buffer fill,
|
||||||
// expect this to stay "n/a" in practice.
|
// when the radar is in Engineering Mode (per-gate energies). If
|
||||||
hrBpmMm.textContent = ' n/a ';
|
// the module fell back to Normal Mode (ASCII distance) HR will
|
||||||
|
// stay missing.
|
||||||
|
hrBpmMm.textContent = ' — BPM ';
|
||||||
hrBpmMm.style.color = '';
|
hrBpmMm.style.color = '';
|
||||||
hrConfMm.textContent = '·';
|
hrConfMm.textContent = '·';
|
||||||
}
|
}
|
||||||
} catch (_) {
|
} catch (_) {
|
||||||
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
||||||
hrBpmMm.textContent = ' n/a '; hrConfMm.textContent = '·';
|
hrBpmMm.textContent = ' — BPM '; hrConfMm.textContent = '·';
|
||||||
} finally { mmVitalsBusy = false; }
|
} finally { mmVitalsBusy = false; }
|
||||||
}
|
}
|
||||||
pollMmwaveVitals();
|
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
|
//! Auxiliary range + per-range-gate vitals modality, attached over a
|
||||||
//! bridge. The module ships factory firmware that emits ASCII
|
//! CP2102 USB-UART bridge.
|
||||||
//! `distance:<cm>\r\n` lines @ 115200 baud, ~6 Hz, in Normal Mode.
|
//!
|
||||||
|
//! 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
|
//! This reader runs in a dedicated thread (blocking serial I/O is
|
||||||
//! awkward inside tokio) and pushes the latest reading + monotonic
|
//! awkward inside tokio) and pushes the latest distance, the per-gate
|
||||||
//! timestamp into a global `OnceLock<Mutex<…>>` that the broadcast
|
//! energy snapshot, and the computed VitalSigns into global
|
||||||
//! tick task reads.
|
//! `OnceLock<Mutex<…>>` slots that the broadcast tick task reads.
|
||||||
//!
|
//!
|
||||||
//! Cold-start tolerance: if the port cannot be opened, the thread
|
//! Cold-start tolerance: if the port cannot be opened, the thread
|
||||||
//! logs once and exits cleanly — the server keeps running with WiFi
|
//! logs once and exits cleanly — the server keeps running with WiFi
|
||||||
//! sensing only. No panics, no retries (operator can hot-plug; if
|
//! sensing only. If config commands fail, we fall back to the ASCII
|
||||||
//! they want auto-reconnect we can add it later).
|
//! 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::sync::{Mutex, OnceLock};
|
||||||
use std::time::{Duration, Instant};
|
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.
|
/// the breathing band (0.1–0.5 Hz) sits comfortably inside Nyquist.
|
||||||
const MMWAVE_SAMPLE_RATE_HZ: f64 = 6.0;
|
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.
|
/// Latest mmWave reading + when it landed.
|
||||||
#[derive(Debug, Clone, Copy)]
|
#[derive(Debug, Clone, Copy)]
|
||||||
pub struct MmwaveReading {
|
pub struct MmwaveReading {
|
||||||
|
|
@ -32,47 +87,80 @@ pub struct MmwaveReading {
|
||||||
pub at: Instant,
|
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 LATEST: OnceLock<Mutex<Option<MmwaveReading>>> = OnceLock::new();
|
||||||
|
static GATES: OnceLock<Mutex<Option<GateSnapshot>>> = OnceLock::new();
|
||||||
static VITALS: OnceLock<Mutex<Option<VitalSigns>>> = 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>> {
|
fn latest() -> &'static Mutex<Option<MmwaveReading>> {
|
||||||
LATEST.get_or_init(|| Mutex::new(None))
|
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>> {
|
fn vitals_slot() -> &'static Mutex<Option<VitalSigns>> {
|
||||||
VITALS.get_or_init(|| Mutex::new(None))
|
VITALS.get_or_init(|| Mutex::new(None))
|
||||||
}
|
}
|
||||||
|
|
||||||
fn detector_slot() -> &'static Mutex<VitalSignDetector> {
|
fn br_detector() -> &'static Mutex<VitalSignDetector> {
|
||||||
DETECTOR.get_or_init(|| Mutex::new(VitalSignDetector::new(MMWAVE_SAMPLE_RATE_HZ)))
|
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> {
|
pub fn current(staleness: Duration) -> Option<MmwaveReading> {
|
||||||
let g = latest().lock().unwrap();
|
let g = latest().lock().unwrap();
|
||||||
let r = (*g)?;
|
let r = (*g)?;
|
||||||
if r.at.elapsed() <= staleness { Some(r) } else { None }
|
if r.at.elapsed() <= staleness { Some(r) } else { None }
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Returns the latest mmWave-derived VitalSigns. Breathing is
|
/// Returns the latest per-gate energy snapshot, gated on freshness.
|
||||||
/// computed from a 30-s buffer of distance samples (chest movement
|
pub fn current_gates(staleness: Duration) -> Option<GateSnapshot> {
|
||||||
/// modulates range by 5–10 mm — visible as flicker between adjacent
|
let g = gates_slot().lock().unwrap();
|
||||||
/// cm bins). Heart rate at 6 Hz / cm precision is essentially below
|
let s = (*g).clone()?;
|
||||||
/// the noise floor; we surface it but expect very low confidence.
|
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
|
/// Returns `None` if the most recent distance reading is older than
|
||||||
/// `staleness` (so the UI can show "—" when the radar is unplugged).
|
/// `staleness`.
|
||||||
pub fn current_vitals(staleness: Duration) -> Option<VitalSigns> {
|
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)?;
|
current(staleness)?;
|
||||||
vitals_slot().lock().unwrap().clone()
|
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) {
|
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();
|
let (br_samples, br_cap, _hr_samples, _hr_cap) = det.buffer_status();
|
||||||
(br_samples, br_cap)
|
(br_samples, br_cap)
|
||||||
}
|
}
|
||||||
|
|
@ -87,6 +175,52 @@ pub fn spawn_reader(port: String, baud: u32) {
|
||||||
.expect("failed to spawn mmwave-reader thread");
|
.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) {
|
fn run(port: String, baud: u32) {
|
||||||
let mut serial = match serialport::new(&port, baud)
|
let mut serial = match serialport::new(&port, baud)
|
||||||
.timeout(Duration::from_millis(500))
|
.timeout(Duration::from_millis(500))
|
||||||
|
|
@ -102,49 +236,360 @@ fn run(port: String, baud: u32) {
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
let mut buf = Vec::with_capacity(256);
|
// Try to switch into Engineering Mode. If it fails the loop below
|
||||||
let mut tmp = [0u8; 128];
|
// 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 {
|
loop {
|
||||||
match serial.read(&mut tmp) {
|
match serial.read(&mut tmp) {
|
||||||
Ok(0) => continue,
|
Ok(0) => continue,
|
||||||
Ok(n) => buf.extend_from_slice(&tmp[..n]),
|
Ok(n) => buf.extend_from_slice(&tmp[..n]),
|
||||||
Err(e) => {
|
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}");
|
tracing::warn!("ADR-121 mmWave reader: read error: {e}");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Drain complete lines.
|
|
||||||
while let Some(pos) = buf.iter().position(|&b| b == b'\n') {
|
if engineering_mode {
|
||||||
let raw_line: Vec<u8> = buf.drain(..=pos).collect();
|
// Binary frames only. The engineering payload contains
|
||||||
let line = String::from_utf8_lossy(&raw_line).trim().to_string();
|
// arbitrary bytes (including 0x0A), so we must NOT run the
|
||||||
if let Some(cm) = parse_distance(&line) {
|
// ASCII line-drain or it will chop partial frames in half
|
||||||
*latest().lock().unwrap() = Some(MmwaveReading {
|
// mid-buffer. Lesson learned: at 6 Hz × 141 bytes the
|
||||||
distance_cm: cm,
|
// tail of every read is usually a partial frame, and the
|
||||||
at: Instant::now(),
|
// ASCII drain destroyed ~80 % of them in our first cut.
|
||||||
});
|
consume_binary_frames(&mut buf);
|
||||||
// Feed the same value into a VitalSignDetector tuned for
|
} else {
|
||||||
// mmWave's 6 Hz cadence so we can publish a
|
// Normal Mode fallback: ASCII `distance:NNN\r\n` lines.
|
||||||
// breathing-rate estimate from chest-induced cm
|
while let Some(pos) = buf.iter().position(|&b| b == b'\n') {
|
||||||
// flicker. Phase is empty (radar gives no phase here)
|
let raw_line: Vec<u8> = buf.drain(..=pos).collect();
|
||||||
// — extract_heartbeat falls back to amplitude residual
|
let line = String::from_utf8_lossy(&raw_line).trim().to_string();
|
||||||
// which is mostly noise at cm precision, but we
|
if let Some(cm) = parse_distance(&line) {
|
||||||
// surface it anyway for transparency.
|
ingest_distance(cm);
|
||||||
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:?}");
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// 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).
|
/// Parse `distance:<digits>` (HLK-LD2402 Normal Mode line format).
|
||||||
pub fn parse_distance(line: &str) -> Option<u32> {
|
pub fn parse_distance(line: &str) -> Option<u32> {
|
||||||
let lower = line.trim().to_ascii_lowercase();
|
let lower = line.trim().to_ascii_lowercase();
|
||||||
|
|
@ -165,4 +610,40 @@ mod tests {
|
||||||
assert_eq!(parse_distance(""), None);
|
assert_eq!(parse_distance(""), None);
|
||||||
assert_eq!(parse_distance("distance:abc"), 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>
|
||||||
<span class="pill" id="hrPill"
|
<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);"
|
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.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.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>
|
<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' : '';
|
hrBpmMm.style.color = out ? '#f0a020' : '';
|
||||||
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
hrConfMm.textContent = '· ' + ((vs.heartbeat_confidence || 0) * 100).toFixed(0) + '%';
|
||||||
} else {
|
} else {
|
||||||
// mmWave at 6 Hz / cm precision can't resolve heartbeat —
|
// ADR-122: heart rate becomes available after ~30 s buffer fill,
|
||||||
// expect this to stay "n/a" in practice.
|
// when the radar is in Engineering Mode (per-gate energies). If
|
||||||
hrBpmMm.textContent = ' n/a ';
|
// the module fell back to Normal Mode (ASCII distance) HR will
|
||||||
|
// stay missing.
|
||||||
|
hrBpmMm.textContent = ' — BPM ';
|
||||||
hrBpmMm.style.color = '';
|
hrBpmMm.style.color = '';
|
||||||
hrConfMm.textContent = '·';
|
hrConfMm.textContent = '·';
|
||||||
}
|
}
|
||||||
} catch (_) {
|
} catch (_) {
|
||||||
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
brBpmMm.textContent = ' — BPM '; brConfMm.textContent = '·';
|
||||||
hrBpmMm.textContent = ' n/a '; hrConfMm.textContent = '·';
|
hrBpmMm.textContent = ' — BPM '; hrConfMm.textContent = '·';
|
||||||
} finally { mmVitalsBusy = false; }
|
} finally { mmVitalsBusy = false; }
|
||||||
}
|
}
|
||||||
pollMmwaveVitals();
|
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