diff --git a/.github/workflows/cmake-multi-platform.yml b/.github/workflows/cmake-multi-platform.yml index 842cfa0..e4e0f1e 100644 --- a/.github/workflows/cmake-multi-platform.yml +++ b/.github/workflows/cmake-multi-platform.yml @@ -146,7 +146,7 @@ jobs: run: > cmake --build build --target WiFiDriver StreamTxDemo StreamDuplexDemo StreamStdinSelftest - ToneMaskSelftest + ToneMaskSelftest BfReportDecodeSelftest - name: Test working-directory: build diff --git a/CMakeLists.txt b/CMakeLists.txt index c68d0f0..6b13328 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -266,6 +266,15 @@ add_executable(StreamDuplexDemo target_link_libraries(StreamDuplexDemo PUBLIC WiFiDriver PRIVATE PkgConfig::libusb) target_include_directories(StreamDuplexDemo PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/txdemo) +# WiFiSenseDemo — Wi-Fi motion/presence sensor from beamforming reports. Drives +# two adapters (a sounder + a beamformee) on one host, self-captures the reports, +# decodes the per-tone Givens angles (src/BfReportDecode.h), and shows a live +# motion readout. See examples/sense/README.md and docs/beamforming-victim-sensing.md. +add_executable(WiFiSenseDemo + examples/sense/main.cpp +) +target_link_libraries(WiFiSenseDemo PUBLIC WiFiDriver PRIVATE PkgConfig::libusb) + # Headless regression guard for the binary-stdin framing shared by the two # stream demos above (txdemo/stream_stdin.h). No libusb, no radio — just the # set_stdin_binary() + read_exact() path, so a text-mode regression (e.g. the @@ -294,3 +303,14 @@ add_executable(ToneMaskSelftest target_link_libraries(ToneMaskSelftest PRIVATE WiFiDriver) add_test(NAME tone_mask_math COMMAND ToneMaskSelftest) + +# Headless guard for the beamforming-report decoder (src/BfReportDecode.h) — the +# LSB-first Givens-angle unpacking + per-tone variance behind WiFiSenseDemo. It +# decodes a real captured VHT MU report and checks the angles against an offline +# reference, so a port regression fails `ctest` instead of only on a radio. +add_executable(BfReportDecodeSelftest + examples/sense/bf_report_decode_selftest.cpp +) +target_link_libraries(BfReportDecodeSelftest PRIVATE WiFiDriver) + +add_test(NAME bf_report_decode COMMAND BfReportDecodeSelftest) diff --git a/docs/beamforming-victim-sensing.md b/docs/beamforming-victim-sensing.md index a4bd8bb..720f8a9 100644 --- a/docs/beamforming-victim-sensing.md +++ b/docs/beamforming-victim-sensing.md @@ -225,6 +225,29 @@ tools/bf_report_decode.py cap.raw --csv cap.csv # interference: per-tone cross-frame variance of psi (Result 2/3) ``` +## Try it — `WiFiSenseDemo` + +`WiFiSenseDemo` turns the above into a runnable motion/presence sensor: one binary +drives two adapters — a sounder and a beamformee — sounds continuously, +self-captures the reports, decodes the per-tone angles in C++ +(`src/BfReportDecode.h`), and prints a live readout (the mean per-tone cross-frame +variance of the phase angle) against a **self-calibrating noise floor** — a +CFAR-style test shown as `σ`, with `CLEAR` / `MOTION` verdicts. + +```sh +WiFiSenseDemo --channel 6 \ + --sounder 0x0bda:0x8812 --beamformee 0x0bda:0xc812 +``` + +Give it a few seconds to calibrate, then move near the adapters — the `σ` reading +climbs and the verdict flips to `MOTION`. The effect is strongest with the two +adapters **physically separated**; side by side they share an almost-static short +channel a hand barely perturbs. + +**Full hands-on guide — hardware, placement, all the knobs, the decoding/detector +maths, and a capture→analyse loop for trying your own formulas — is in +[`examples/sense/README.md`](../examples/sense/README.md).** + ## Related - [beamforming-self-sounding.md](beamforming-self-sounding.md) — the sounding diff --git a/examples/sense/README.md b/examples/sense/README.md new file mode 100644 index 0000000..da37475 --- /dev/null +++ b/examples/sense/README.md @@ -0,0 +1,325 @@ +# WiFiSenseDemo — Wi-Fi motion sensing you can run on your own dongles + +A runnable example built on the `devourer` library that turns two cheap Realtek +Wi-Fi adapters into a **motion / presence sensor**. No SDR, no special AP — one +adapter sounds, the other answers, and the demo reads human motion out of the +802.11ac beamforming feedback the second adapter returns. + +This document is written so you can **reproduce it, move it into your own +environment, and change the maths**. If you only want the theory of *why* a +beamforming report senses motion, read +[`docs/beamforming-victim-sensing.md`](../../docs/beamforming-victim-sensing.md); +this file is the hands-on half. + +--- + +## 1. The one-paragraph idea + +An 802.11ac beamformer (the *sounder*) asks a *beamformee* to measure the +per-subcarrier channel between the sounder's two transmit antennas and report it +back, compressed as **Givens rotation angles** (a phase `phi` and an amplitude +angle `psi` per subcarrier). That report is a measurement *taken at the +beamformee* of the radio channel between the two devices. When a person moves in +that channel, the multipath changes, so the reported angles **jitter frame to +frame**. Measure that jitter and you have a motion detector. This is the +mechanism behind published work such as Wi-BFI, BeamSense and BFMSense. + +The demo drives *both* ends itself — a sounder and a beamformee on the same host +— so you don't need a cooperating AP. The sounder injects a short NDPA frame; the +chip hardware-generates the sounding NDP; the beamformee answers with a +compressed beamforming report; the sounder self-captures it on a concurrent RX +loop. All of that is the `devourer` beamforming self-sounding path (see +[`docs/beamforming-self-sounding.md`](../../docs/beamforming-self-sounding.md)). + +--- + +## 2. Hardware you need + +**Two USB adapters** that `devourer` supports, plugged into the same host: + +| Role | Requirement | Good choice | +|------|-------------|-------------| +| **Sounder** | Can transmit + self-capture (TX-with-RX). Any supported generation works; Jaguar3 (8822CU `0bda:c812`, 8822EU `0bda:a81a`) is the best-tested. | RTL8822CU | +| **Beamformee** | Must answer a VHT sounding with a compressed report. **Two receive antennas (2T2R) give a far cleaner signal** than a 1T1R part. | RTL8822BU / RTL8822CU (2T2R) | + +A 1T1R beamformee (e.g. an 8811AU) *works* but produces a much noisier, weaker +signal — its "relative channel between two antennas" is degenerate. If your +readout is jumpy, suspect the beamformee first. + +**Placement matters more than anything else** — see §6. + +The demo is built on macOS/Linux (libusb). It does not need root on macOS; on +Linux you may need to run as root or add a udev rule so libusb can claim the +interface. + +--- + +## 3. Build + +From the repo root: + +```sh +cmake -S . -B build +cmake --build build -j --target WiFiSenseDemo +``` + +The decoder has a headless self-test that runs under `ctest` (no radio needed): + +```sh +ctest --test-dir build -R bf_report_decode +``` + +--- + +## 4. Run it + +```sh +./build/WiFiSenseDemo --channel 6 \ + --sounder 0x0bda:0xc812 \ + --beamformee 0x0bda:0xb82c +``` + +Both selectors take `[VID:]PID` (VID defaults to `0x0bda`). Find your adapters' +IDs with `lsusb` (Linux) or `system_profiler SPUSBDataType` / `ioreg -p IOUSB` +(macOS). + +Startup takes a few seconds: bring up both radios → calibrate the decoder split +(~48 reports) → acquire the noise floor (~2.5 s). Then you get a live line: + +``` + [ CLEAR ] 0.4σ | | now 0.0003 base 0.0003 1310/s + [ MOTION ] 9.2σ |####################| now 0.0016 base 0.0003 1298/s +``` + +Wave your hand near the adapters and the `σ` reading climbs; the verdict flips to +`MOTION` and holds for ~1 s after you stop. `Ctrl-C` to quit. + +### Reading the line + +| Field | Meaning | +|-------|---------| +| `CLEAR` / `MOTION` | verdict. `MOTION·nb` = the rise is concentrated on a few tones (looks like a narrowband interferer, not broadband motion). | +| `σ` | how many noise-floor standard deviations the current energy sits above the self-calibrated floor. The detector fires at `σ ≈ k` (see §7). | +| bar | `σ` as a bar, saturating at 10σ. | +| `now` | current motion energy (mean per-tone circular variance of `phi`). | +| `base` | the self-calibrated still-floor it is measured against. | +| `N/s` | beamforming reports captured per second (health indicator; a fast Jaguar3 sounder gives ~1000–1500/s). | + +--- + +## 5. All the knobs + +**Command-line flags** + +| Flag | Default | Purpose | +|------|---------|---------| +| `--channel N` | `6` | Wi-Fi channel to sound on. Try 5 GHz (36, 149) for a different multipath environment. | +| `--sounder [VID:]PID` | `0bda:8812` | sounder adapter selector | +| `--beamformee [VID:]PID` | `0bda:c812` | beamformee adapter selector | +| `--vid 0xNNNN` | `0x0bda` | default VID for both selectors (for OEM-rebadged dongles) | +| `--sensitivity low\|med\|high` | `med` | detector threshold `k` = 6 / 4 / 2.5 sigmas | +| `-v`, `--verbose` | off | show the library's bring-up logs (otherwise quieted to warnings) | + +**Environment variables** + +| Var | Purpose | +|-----|---------| +| `DEVOURER_SENSE_K=` | override the CFAR threshold `k` directly (finer than `--sensitivity`). | +| `DEVOURER_SENSE_DUMP=1` | print every captured report as a `HEX` line on **stderr** — the input for offline analysis (§8). | +| `DEVOURER_SENSE_DEBUG=1` | print the first few captured reports' geometry (SA, Nc/Nr, MU, Ns) for a sanity check. | + +--- + +## 6. Environment — the single biggest lever + +The signal strength depends almost entirely on **how much a moving person changes +the channel between the two adapters, relative to the static part of that +channel.** + +- **Separate the two adapters.** Two dongles side by side on the same hub share a + strong, short, line-of-sight channel that a hand barely perturbs. Put one on a + **USB extension a metre or more away**, ideally across the space you want to + sense. This can turn a marginal signal into an obvious one. +- **Put the person in the path.** Motion *between* or *near* the two antennas + moves the needle most. +- **Multipath helps.** A reflective room (walls, furniture) gives the channel more + structure to perturb than an anechoic free-space shot. +- **Channel width / band.** 20 MHz on 2.4 GHz is the default. 5 GHz and wider + channels change the coherence bandwidth and the per-tone structure; worth + experimenting. + +Reference numbers from one indoor 20 MHz / channel-6 setup (2T2R↔1T1R, adapters +~30 cm apart): + +| condition | motion energy (mean per-tone circular variance of `phi`) | +|-----------|----------------------------------------------------------| +| still | 0.0003 (floor), window-to-window jitter ~0.00002 | +| hand wave next to a dongle | 0.0006 – 0.002 (2–6× the floor) | + +Your absolute numbers **will differ** — that is exactly why the detector +self-calibrates rather than using a fixed threshold. Use these only as a rough +scale. + +--- + +## 7. How it works inside (so you can change the maths) + +Everything below lives in two files: + +- **`src/BfReportDecode.h`** — the report decoder + the `MotionMeter` metric. +- **`examples/sense/main.cpp`** — the `AdaptiveDetector` + display. + +### 7a. Decoding the report → angles + +`parse_report()` matches a VHT/HT compressed beamforming report and locates the +packed angle bits. `decode_angles()` unpacks, per subcarrier, one `phi` (phase) +and one `psi` (amplitude) angle, LSB-first, and dequantises them: + +``` +phi = (2q + 1) · π / 2^b_phi # dequant_phi +psi = (2q + 1) · π / 2^(b_psi + 2) # dequant_psi +``` + +**The bit split `(b_phi, b_psi)` matters and is easy to get wrong.** The 802.11 +compressed-Givens codebook always pairs `b_phi = b_psi + 2`. For the common +10-bits-per-subcarrier 2×1 report that uniquely means **`(6, 4)`**. `pick_split()` +enforces that relationship — do **not** replace it with a naive "minimise +cross-frame variance" search: a too-coarse `psi` (e.g. 2 bits) is trivially +constant, so an unconstrained search picks a bit-*misaligned* split whose `phi` +decodes to garbage that jitters on a static channel and reads as constant motion. +This was a real bug; the self-test now pins the split to `(6,4)`. + +### 7b. The motion metric (`MotionMeter`) + +For each report we keep the first `phi` of every subcarrier in a sliding window +(`kWindow = 512` reports, ~0.3–0.5 s). The per-tone motion signal is the +**circular variance** of that phase over the window: + +``` +var[k] = 1 − |mean_over_window( e^{i·phi_k} )| # in [0, 1] +motion_energy = mean_k var[k] +``` + +Circular variance is 0 when a tone's phase is constant (static channel) and rises +toward 1 as it scatters (a changing channel). It is CFO-robust in the sense that +a *constant* phase offset cancels; what survives is frame-to-frame change. + +Why `phi` (phase) and not `psi` (amplitude)? Measured: a hand wave moves the +phase but barely moves the coarse 4-bit amplitude ratio, so `phi` is the sensitive +signal. `psi` is decoded too, and the "broadband vs localized" flag +(`MotionMeter::localized()`) uses the per-tone shape to distinguish human motion +from a narrowband interferer. + +**Things to try here:** a different aggregation (median/percentile instead of +mean over tones), a frame-to-frame `|Δphi|` "velocity" metric, a shorter window +for faster response, or weighting tones by their SNR. + +### 7c. The detector (`AdaptiveDetector`) + +A fixed threshold is wrong because the still-floor varies with hardware, channel +and geometry. Instead the detector self-calibrates (a CFAR-style test): + +- Track the **floor** (mean still energy) and its **jitter** `dev` with a + rate-independent EMA (`kTauFloor = 4 s`; faster `kWarmTau = 0.4 s` during a + `kWarmSec = 2.5 s` warm-up). The floor **freezes while motion is held**, so a + moving subject can't drag it up and blind the detector. +- Threshold: `floor + k · max(dev, kDevFloor)`. `k` is the sensitivity + (`--sensitivity`/`DEVOURER_SENSE_K`); `kDevFloor` is a minimum jitter so the + threshold can't collapse onto a perfectly-still floor. +- **Hysteresis:** arm only after the energy stays over threshold for + `kArmSec = 0.15 s` (a lone spike can't trigger), then **hold** `kHoldSec = 1.2 s` + after it drops (no flicker; bridges the gaps in an intermittent wave). +- `σ` shown in the UI is `(energy − floor) / max(dev, kDevFloor)`. + +The tunable constants are all `static constexpr` at the top of +`AdaptiveDetector` in `examples/sense/main.cpp`: + +``` +kWindow = 512 # metric window (reports) [in main.cpp top-level] +kWarmSec = 2.5 s # floor acquisition +kWarmTau = 0.4 s # fast tracking during warm-up +kTauFloor = 4.0 s # floor/jitter tracking constant +kArmSec = 0.15 s # dwell over threshold before MOTION +kHoldSec = 1.2 s # hold MOTION after last trigger +kDevSeed = 0.00005 # initial jitter estimate +kDevFloor = 0.00004 # minimum jitter → minimum sensitivity margin +``` + +If your still-floor sits at a different scale than the reference in §6, the two +numbers most likely to need adjusting are **`kDevFloor`** (raise it if you get +false alarms at rest, lower it if real motion never crosses the threshold) and +**`k`** (via `DEVOURER_SENSE_K`, no rebuild needed). + +--- + +## 8. Experiment with your own formulas (capture → analyse loop) + +You do not have to edit C++ to try new maths. Capture the raw reports and analyse +them offline: + +```sh +# capture ~30 s of reports to a file (stderr carries the raw dump) +DEVOURER_SENSE_DUMP=1 ./build/WiFiSenseDemo --channel 6 \ + --sounder 0x0bda:0xc812 --beamformee 0x0bda:0xb82c \ + 2> capture.txt + +# decode + inspect with the reference Python tool +grep '' capture.txt | tools/bf_report_decode.py +``` + +`tools/bf_report_decode.py` prints the header (Nc/Nr/BW/Ng), the chosen split, +per-stream SNR and the per-tone `|h_B/h_A|`. From the same `capture.txt` you can +compute anything you like in a few lines of Python — per-tone variance, a +different window, a spectrogram of `phi[k]` over time, a doppler estimate — and +label a run by doing a clean **still segment then a moving segment** and comparing +the two. That is exactly how the metric, window and thresholds in this demo were +chosen. + +> **A note on the reference tool:** `tools/bf_report_decode.py` currently selects +> the split by cross-frame stability and can land on the degenerate `(8,2)` for a +> 10-bit report. When comparing against the C++ path, force the correct Givens +> split `(6,4)` (see §7a). + +--- + +## 9. Limitations and honest caveats + +- **Weak coupling on close adapters.** Side-by-side dongles barely see motion. §6 + is not optional advice — it is the difference between working and not. +- **Coarse amplitude.** The Realtek compact codebook gives `psi` only ~4 bits, so + amplitude-based sensing is limited; this demo leans on phase. +- **Second-adapter flakiness.** Some cheap beamformees' firmware crashes on long + runs and the adapter drops off the USB bus. The demo has a stall watchdog (§10); + if it fires, replug that adapter. +- **A single-radio (`--mode self`) variant** — the sounder acting as its own + beamformee — is plausible and would remove the flaky second adapter, but is not + implemented here. +- **No passive/AP-sniffing mode.** Sensing an existing AP↔client sounding exchange + (the Wi-BFI approach) is a natural extension but is deliberately **not** shipped: + it needs an AP actively sounding VHT beamforming, which we could not validate + against. Contributions welcome. + +--- + +## 10. Troubleshooting + +| Symptom | Cause / fix | +|---------|-------------| +| `could not open VVVV:PPPP` | Adapter not found. Check `lsusb`. A prior hang can drop an adapter off the bus — **unplug/replug it**. | +| Stuck on `calibrating decoder… 0 reports (0/s)` | The beamformee isn't answering: wrong PID, it doesn't support VHT sounding, or it fell off the bus. Try `-v` to see bring-up, and confirm both adapters enumerate. | +| `report stream stalled Ns — beamformee stopped responding` | The beamformee firmware crashed. The demo stops cleanly; replug that adapter before re-running. | +| Constant `MOTION` at rest | Threshold too low for your floor: raise `DEVOURER_SENSE_K`, or `--sensitivity low`. If `base` reads `0.000` and never rises, that's the old split bug — make sure you're on a build with the `(6,4)` `pick_split`. | +| Never triggers even on a strong wave | Threshold too high, or coupling too weak. `--sensitivity high`, and **separate the adapters** (§6). | +| Needs root on Linux | libusb can't claim the interface. Run as root or add a udev rule for the adapter's VID:PID. | + +--- + +## 11. Files + +| File | What | +|------|------| +| `examples/sense/main.cpp` | the `WiFiSenseDemo` binary: adapter bring-up, sounding loop, `AdaptiveDetector`, display, watchdog. | +| `src/BfReportDecode.h` | header-only report decoder + `MotionMeter` (reusable; no libusb dependency). | +| `examples/sense/bf_report_decode_selftest.cpp` | headless `ctest` that guards the decoder against a real captured report. | +| `docs/beamforming-victim-sensing.md` | the theory: why a beamforming report measures the channel and senses motion. | +| `tools/bf_report_decode.py` | reference Python decoder for offline analysis. | diff --git a/examples/sense/bf_report_decode_selftest.cpp b/examples/sense/bf_report_decode_selftest.cpp new file mode 100644 index 0000000..726ea0c --- /dev/null +++ b/examples/sense/bf_report_decode_selftest.cpp @@ -0,0 +1,111 @@ +/* Headless self-test for src/BfReportDecode.h — guards the C++ port of + * tools/bf_report_decode.py against regressions. Registered as a ctest, so a + * decode break fails CI instead of only surfacing on a radio. + * + * Checks: the LSB-first BitReader and the dequant formulas on known inputs, the + * report header parse + fixed-split angle decode against a real captured MU + * report (reference psi values computed offline with a fixed (8,2) split), and + * that the split picker returns a valid split. */ +#include "BfReportDecode.h" + +#include +#include +#include +#include +#include + +using namespace devourer::bf; + +static int failures = 0; +#define CHECK(cond, msg) \ + do { \ + if (!(cond)) { \ + std::printf("FAIL: %s\n", msg); \ + ++failures; \ + } \ + } while (0) + +static bool approx(double a, double b, double tol = 1e-4) { + return std::fabs(a - b) < tol; +} + +static std::vector from_hex(const std::string &h) { + std::vector out; + for (size_t i = 0; i + 1 < h.size(); i += 2) + out.push_back((uint8_t)std::strtoul(h.substr(i, 2).c_str(), nullptr, 16)); + return out; +} + +/* A real VHT MU Compressed Beamforming report captured from an 8822CU beamformee + * (20 MHz, Nr=2 Nc=1, MU). psi[0..5] below were computed offline from these bytes + * with a fixed (b_phi=8, b_psi=2) split. */ +static const char *kReportHex = + "e000000056427505d60000e04c8822ce00000000000010001500088c04c2a98dad97b09fbe" + "addaadc7bfccbde1c9e5cfe8cdf3cdf1d1eacfe5cff0d5eed9f0d9e6e1ffd70cd820e017d2" + "25d61ef093f0b9daa1ec91e687dc83e093e058f417ecfbebf9ebe9e1f1db03dc08de0dda11" + "d814de0fd808d60dd219c815c605b811b01bac20b62ac40f01f00fef00100100ff0011111001cbbf1bd5"; + +int main() { + /* 1. BitReader — LSB-first. byte 0xB4 = 1011 0100b; reading 3 bits gives the + * low three bits in LSB order = 0b100 = 4; next 5 bits = 0b10110 = 22. */ + { + uint8_t bytes[2] = {0xB4, 0x00}; + BitReader br(bytes, 2); + CHECK(br.read(3) == 4u, "BitReader low 3 bits"); + CHECK(br.read(5) == 22u, "BitReader next 5 bits"); + } + + /* 2. dequant — psi=(2q+1)pi/2^(b+2), phi=(2q+1)pi/2^b. */ + CHECK(approx(dequant_psi(0, 2), M_PI / 16.0), "dequant_psi q0 b2"); + CHECK(approx(dequant_psi(3, 2), 7.0 * M_PI / 16.0), "dequant_psi q3 b2"); + CHECK(approx(dequant_phi(0, 8), M_PI / 256.0), "dequant_phi q0 b8"); + + /* 3. parse_report on the real MU report. */ + std::vector frame = from_hex(kReportHex); + ReportHdr hdr; + CHECK(parse_report(frame.data(), frame.size(), hdr), "parse_report matches"); + CHECK(hdr.nc == 1 && hdr.nr == 2, "nc/nr"); + CHECK(hdr.bw == 0 && hdr.ng == 1, "bw/ng"); + CHECK(hdr.mu && hdr.vht, "mu/vht"); + CHECK(hdr.ns == 52, "ns=52"); + CHECK(hdr.per_sc_bits == 10, "per_sc_bits=10 (MU)"); + CHECK(hdr.angle_len == 65, "angle_len=65 (52*10 bits)"); + + /* 4. fixed-split decode vs offline reference. */ + std::vector psi; + CHECK(decode_psi(hdr, 8, 2, psi), "decode_psi ok"); + CHECK(psi.size() == 52, "52 psi values"); + const double ref[6] = {0.589049, 1.374447, 0.589049, + 0.981748, 0.981748, 1.374447}; + for (int k = 0; k < 6; ++k) + CHECK(approx(psi[k], ref[k]), "psi matches reference"); + bool in_range = true; + for (double p : psi) + if (p < 0.0 || p > M_PI / 2.0) + in_range = false; + CHECK(in_range, "all psi in (0, pi/2)"); + + /* 5. split picker returns the standard Givens split (b_phi = b_psi + 2), + * i.e. (6,4) for this 10-bit/tone 2x1 report — NOT the degenerate (8,2) that a + * naive min-variance search picks because a 2-bit psi is trivially constant. */ + { + std::vector batch(8, hdr); /* same frame repeated is enough */ + int bphi = 0, bpsi = 0; + CHECK(pick_split(batch, bphi, bpsi), "pick_split found a split"); + CHECK(bphi + bpsi == hdr.per_sc_bits, "split sums to per_sc_bits"); + CHECK(bphi == bpsi + 2, "split obeys Givens b_phi = b_psi + 2"); + CHECK(bphi == 6 && bpsi == 4, "10-bit/tone 2x1 split is (6,4)"); + } + + /* 6. MotionMeter — identical reports => ~zero motion energy. */ + { + MotionMeter m(52, 8, 2, 16); + for (int i = 0; i < 8; ++i) + m.push(hdr); + CHECK(m.motion_energy() < 1e-9, "static channel => zero motion energy"); + } + + if (failures == 0) + std::printf("bf_report_decode_selftest: all checks passed\n"); + return failures == 0 ? 0 : 1; +} diff --git a/examples/sense/main.cpp b/examples/sense/main.cpp new file mode 100644 index 0000000..a9d3636 --- /dev/null +++ b/examples/sense/main.cpp @@ -0,0 +1,534 @@ +/* WiFiSenseDemo — a runnable Wi-Fi motion/presence sensor built on devourer. + * + * An 802.11ac beamforming report is a measurement taken at the beamformee: its + * per-tone Givens angles track the channel, so a moving person perturbs them + * frame-to-frame. This demo captures reports, decodes them (src/BfReportDecode.h), + * and shows a live motion readout — the per-tone cross-frame variance of the + * phase angle. See docs/beamforming-victim-sensing.md. + * + * One binary drives TWO adapters: a sounder that injects NDPAs (the MAC + * hardware-generates the NDP) and self-captures the reports, and a beamformee + * that responds in hardware. Two dongles; the effect is stronger when they are + * physically separated (a static short channel barely moves). + */ +#ifdef _WIN32 +#define NOMINMAX /* keep windows.h (via libusb.h) from defining min/max macros */ +#endif + +#if defined(__ANDROID__) || defined(_MSC_VER) || defined(__APPLE__) +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BfReportDecode.h" +#include "RadiotapBuilder.h" +#include "RxPacket.h" +#include "SignalStop.h" +#include "UsbOpen.h" +#include "WiFiDriver.h" +#include "logger.h" + +using devourer::bf::MotionMeter; +using devourer::bf::parse_report; +using devourer::bf::ReportHdr; + +/* Portable environment set (the demo hands arming flags to the library via env): + * POSIX setenv, or _putenv_s on Windows (MSVC + MinGW have no POSIX setenv). */ +static void set_env(const char *name, const char *value) { +#ifdef _WIN32 + _putenv_s(name, value); +#else + ::setenv(name, value, 1); +#endif +} + +/* The sounder's TA and the address a beamformee arms to respond to (matches the + * canonical SA used across devourer's TX path). */ +static const uint8_t kCanonicalSa[6] = {0x57, 0x42, 0x75, 0x05, 0xd6, 0x00}; +static constexpr uint16_t REG_MACID = 0x0610; +static constexpr int kCalReports = 48; /* reports to calibrate the bit split */ +static constexpr size_t kWindow = 512; /* variance window (~0.3 s at a fast + * Jaguar3 sounding rate — long enough + * to span human motion, short enough + * to feel live) */ +static constexpr int kStallSec = 8; /* no reports for this long => the + * beamformee stopped responding; stop + * rather than sound into the void */ + +/* -------------------------------------------------------------- Detector ---- */ +/* Adaptive presence detector. Fed the MotionMeter's energy once per report, it + * self-calibrates a noise floor and fires when the energy rises a configurable + * number of standard deviations above it (a CFAR-style test), then holds the + * verdict for a short time so a moving-then-pausing subject doesn't flicker. + * + * Why not a fixed threshold: the still-state energy floor varies with hardware, + * channel and geometry, so a magic constant tuned on one rig is wrong on the + * next. The floor tracks the quiet state with a rate-independent time constant + * but FREEZES while motion is held — otherwise a subject who keeps moving would + * slowly raise the floor and blind the detector (the classic motion-sensor + * failure). During the initial warm-up the floor latches onto the quietest + * instant seen, so a subject already moving at startup can't set a high floor. */ +class AdaptiveDetector { +public: + explicit AdaptiveDetector(double k) : _k(k) {} + + /* Call only once the MotionMeter window is full, so `energy` is a real + * still-state estimate and not the window-fill transient (which is ~0 and + * would peg the floor at zero -> always-MOTION). */ + void update(double energy, double t) { + _energy = energy; + if (!_init) { + _floor = energy; + _dev = kDevSeed; + _last = t; + _warm_until = t + kWarmSec; + _init = true; + return; + } + double dt = t - _last; + if (dt < 0) + dt = 0; + _last = t; + bool warming = t < _warm_until; + _warm = warming; + /* Track the floor (mean still energy) and its jitter with an EMA — fast + * during warm-up to converge, slow after. Freeze while motion is held so a + * moving subject can't drag the floor up and blind the detector. */ + if (warming || !_active) { + double tau = warming ? kWarmTau : kTauFloor; + double a = 1.0 - std::exp(-dt / tau); + double resid = energy - _floor; + _floor += a * resid; + _dev += a * (std::fabs(resid) - _dev); + } + _thr = _floor + _k * std::max(_dev, kDevFloor); + if (warming) + return; /* no verdict until the floor is acquired */ + /* Hysteresis: arm only after the energy stays over threshold for kArmSec + * (a lone noise spike can't trigger), then hold kHoldSec after it drops (a + * moving-then-pausing subject doesn't flicker). */ + if (energy > _thr) { + if (_over_since < 0.0) + _over_since = t; + if (_active || (t - _over_since) >= kArmSec) { + _active = true; + _hold = kHoldSec; + } + } else { + _over_since = -1.0; + if (_active) { + _hold -= dt; + if (_hold <= 0.0) + _active = false; + } + } + } + + bool active() const { return _active; } + bool warming() const { return _warm; } + double energy() const { return _energy; } + double floor() const { return _floor; } + /* signal strength: how many floor-jitter sigmas the energy sits above the + * floor. The detector fires at sigma == k. */ + double sigma() const { return (_energy - _floor) / std::max(_dev, kDevFloor); } + +private: + static constexpr double kWarmSec = 2.5; /* floor-acquisition window (s) */ + static constexpr double kWarmTau = 0.4; /* fast tracking during warm-up (s) */ + static constexpr double kTauFloor = 4.0; /* floor/jitter tracking constant (s) */ + static constexpr double kArmSec = 0.15; /* dwell over threshold before MOTION (s) */ + static constexpr double kHoldSec = 1.2; /* hold MOTION after last trigger (s) */ + /* Scaled to the measured signal: with the correct (6,4) split the still-channel + * phi circular-variance floor is ~0.0003 and its window-to-window jitter is + * ~0.00002; a hand wave lifts it to ~0.0006–0.002. So the minimum sensitivity + * margin must be tens of micro-units, not milli-units. */ + static constexpr double kDevSeed = 0.00005; /* initial jitter estimate */ + static constexpr double kDevFloor = 0.00004; /* min jitter → min sensitivity margin */ + double _k; + bool _init = false, _active = false, _warm = true; + double _energy = 0, _floor = 0, _dev = 0, _thr = 0; + double _last = 0, _warm_until = 0, _hold = 0, _over_since = -1.0; +}; + +/* ---------------------------------------------------------------- Sensor ---- */ +/* Turns a stream of report frames into a live motion signal. Thread-safe: the + * RX callback calls feed(); the display thread reads the snapshot. */ +class Sensor { +public: + explicit Sensor(double k) : _det(k) {} + + void feed(const uint8_t *frame, size_t n) { + ReportHdr hdr; + if (!parse_report(frame, n, hdr)) + return; + if (std::getenv("DEVOURER_SENSE_DUMP")) { + /* python-tool-compatible raw dump (stderr, so the stdout display is + * untouched): capture with 2>file, analyse with tools/bf_report_decode.py */ + std::fprintf(stderr, ""); + for (size_t i = 0; i < n; ++i) + std::fprintf(stderr, "%02x", frame[i]); + std::fprintf(stderr, "\n"); + } + if (std::getenv("DEVOURER_SENSE_DEBUG")) { + static int dbg = 0; + if (dbg < 8) { + ++dbg; + std::fprintf(stderr, + "[report] len=%zu SA=%02x:%02x:%02x:%02x:%02x:%02x nc=%d " + "nr=%d mu=%d ns=%d\n", + n, frame[10], frame[11], frame[12], frame[13], frame[14], + frame[15], hdr.nc, hdr.nr, hdr.mu, hdr.ns); + } + } + std::lock_guard lk(_mu); + ++_total; + if (!_meter) { + /* calibration: copy full frames until we can pick a stable split */ + _cal.emplace_back(frame, frame + n); + _ns = hdr.ns; + _per = hdr.per_sc_bits; + if ((int)_cal.size() >= kCalReports) + calibrate(); + return; + } + if (hdr.ns != _ns) + return; + if (!_meter->push(hdr)) + return; + /* Wait for a full window before feeding the detector: a partial window + * under-reports circular variance, and that transient would poison the + * self-calibrating floor. */ + if (_meter->count() < kWindow) + return; + _det.update(_meter->motion_energy(), now_sec()); + _localized = _meter->localized(); + } + + struct Snap { + bool calibrated, warming, motion, localized; + double energy, floor, sigma; + long total; + int ns; + }; + Snap snapshot() { + std::lock_guard lk(_mu); + bool cal = _meter != nullptr; + return Snap{cal, cal && _det.warming(), + _det.active(), _localized, + _det.energy(), _det.floor(), + cal ? _det.sigma() : 0.0, + _total, _ns}; + } + +private: + static double now_sec() { + using namespace std::chrono; + return duration_cast>( + steady_clock::now().time_since_epoch()) + .count(); + } + + void calibrate() { + /* re-parse the copies so ReportHdr.angles point into stable storage */ + std::vector batch; + batch.reserve(_cal.size()); + for (auto &f : _cal) { + ReportHdr h; + if (parse_report(f.data(), f.size(), h)) + batch.push_back(h); + } + int bphi = 0, bpsi = 0; + if (!devourer::bf::pick_split(batch, bphi, bpsi)) { + /* fallback for the 2x1 case: the standard Givens split (b_phi = b_psi + 2) + * summing to per_sc_bits — NOT a coarse split like (8,2), whose + * bit-misaligned phi decodes to garbage. */ + bpsi = (_per - 2) / 2; + bphi = bpsi + 2; + } + _bphi = bphi; + _bpsi = bpsi; + _meter = std::make_unique(_ns, bphi, bpsi, kWindow); + _cal.clear(); + _cal.shrink_to_fit(); + } + + std::mutex _mu; + std::vector> _cal; + std::unique_ptr _meter; + AdaptiveDetector _det; + int _ns = 0, _per = 0, _bphi = 0, _bpsi = 0; + bool _localized = false; + long _total = 0; +}; + +/* --------------------------------------------------------------- Display ---- */ +static void run_display(Sensor &sensor) { + using namespace std::chrono; + auto last = steady_clock::now(); + long last_total = 0; + std::printf("\n Wi-Fi motion sensor — move near the adapters to trigger; it " + "stays CLEAR when the room is still.\n" + " (self-calibrating; Ctrl-C to stop)\n\n"); + while (!g_devourer_should_stop) { + std::this_thread::sleep_for(milliseconds(200)); + auto s = sensor.snapshot(); + auto now = steady_clock::now(); + double dt = duration_cast>(now - last).count(); + double rate = dt > 0 ? (s.total - last_total) / dt : 0; + last = now; + last_total = s.total; + if (!s.calibrated) { + std::printf("\r calibrating decoder… %ld reports (%.0f/s) ", + s.total, rate); + std::fflush(stdout); + continue; + } + if (s.warming) { + std::printf("\r acquiring noise floor — one moment… (%.0f rep/s) ", + rate); + std::fflush(stdout); + continue; + } + /* Signal strength = floor-jitter sigmas above the adaptive floor; the + * detector fires around a few sigma. Bar saturates at 10 sigma. */ + double sig = s.sigma; + if (sig < 0) + sig = 0; + int bar = (int)(sig / 10.0 * 40); + if (bar > 40) + bar = 40; + char b[41]; + for (int i = 0; i < 40; ++i) + b[i] = i < bar ? '#' : ' '; + b[40] = 0; + const char *tag = s.motion ? (s.localized ? "MOTION·nb" : " MOTION ") + : " CLEAR "; + std::printf("\r [%s] %5.1fσ |%s| now %.4f base %.4f %.0f/s ", tag, sig, + b, s.energy, s.floor, rate); + std::fflush(stdout); + } + std::printf("\n"); +} + +/* ------------------------------------------------------------ USB helpers --- */ +struct Adapter { + libusb_context *ctx = nullptr; + libusb_device_handle *handle = nullptr; + std::shared_ptr lock; + std::unique_ptr dev; +}; + +/* Open one adapter by VID:PID on its own libusb context, claim + reset, and build + * the device (not yet brought up). Returns false (logged) on failure. */ +static bool open_adapter(Adapter &a, uint16_t vid, uint16_t pid, + const Logger_t &logger) { + if (libusb_init(&a.ctx) < 0) + return false; + a.handle = libusb_open_device_with_vid_pid(a.ctx, vid, pid); + if (!a.handle) { + logger->error("could not open {:04x}:{:04x} — is it plugged in? (a prior " + "hang can drop an adapter off the bus; unplug/replug it)", + vid, pid); + return false; + } + int rc = devourer::claim_interface_then_reset(a.handle, 0, logger, true, a.lock); + if (rc != 0) + return false; + WiFiDriver driver(logger); + a.dev = driver.CreateRtlDevice(a.handle, a.ctx, a.lock); + return a.dev != nullptr; +} + +static bool read_mac(libusb_device_handle *h, uint8_t mac[6]) { + /* REG_MACID is IDR0 (4 bytes @ 0x0610) + IDR4 (2 bytes @ 0x0614). Realtek + * vendor reads are 1/2/4-byte; a single 6-byte read returns garbage, so split + * it the way rtw_read32 + rtw_read16 would. */ + int r1 = libusb_control_transfer(h, 0xC0, 5, REG_MACID, 0, mac, 4, 1000); + int r2 = libusb_control_transfer(h, 0xC0, 5, REG_MACID + 4, 0, mac + 4, 2, 1000); + return r1 == 4 && r2 == 2; +} + +/* ------------------------------------------------------------- active mode -- */ +static int run_active(uint16_t snd_vid, uint16_t snd_pid, uint16_t bfe_vid, + uint16_t bfe_pid, int channel, const Logger_t &logger, + Sensor &sensor) { + /* Beamformee first: arm it (env, read at Init), bring it up on a thread. */ + set_env("DEVOURER_BF_ARM_BFEE", "57:42:75:05:d6:00"); + set_env("DEVOURER_BF_ARM_BFEE_MU", "1"); + Adapter bfe; + if (!open_adapter(bfe, bfe_vid, bfe_pid, logger)) { + logger->error("active: failed to open beamformee {:04x}", bfe_pid); + return 1; + } + std::thread bfe_thread([&bfe, channel]() { + bfe.dev->Init([](const Packet &) {}, /* responds in hardware; RX ignored */ + SelectedChannel{.Channel = (uint8_t)channel, .ChannelOffset = 0, + .ChannelWidth = CHANNEL_WIDTH_20}); + }); + std::this_thread::sleep_for(std::chrono::milliseconds(1500)); /* bring-up + MAC */ + uint8_t bfe_mac[6]; + if (!read_mac(bfe.handle, bfe_mac)) { + logger->error("active: could not read beamformee MAC (REG_MACID)"); + g_devourer_should_stop = true; + bfe_thread.join(); + return 1; + } + logger->info("active: beamformee MAC {:02x}:{:02x}:{:02x}:{:02x}:{:02x}:{:02x}", + bfe_mac[0], bfe_mac[1], bfe_mac[2], bfe_mac[3], bfe_mac[4], + bfe_mac[5]); + + /* Sounder: arm the sounding engine (env, read at InitWrite), VHT2SS_MCS0. + * DEVOURER_TX_WITH_RX=thread must be set BEFORE InitWrite so a Jaguar3 sounder + * keeps its RX filters open for the self-capture (no-op on Jaguar1/2). */ + set_env("DEVOURER_BF_ARM_SOUNDER", "1"); + set_env("DEVOURER_TX_WITH_RX", "thread"); + Adapter snd; + if (!open_adapter(snd, snd_vid, snd_pid, logger)) { + logger->error("active: failed to open sounder {:04x}", snd_pid); + g_devourer_should_stop = true; + bfe_thread.join(); + return 1; + } + snd.dev->InitWrite(SelectedChannel{.Channel = (uint8_t)channel, + .ChannelOffset = 0, + .ChannelWidth = CHANNEL_WIDTH_20}); + snd.dev->SetTxMode(devourer::parse_tx_mode_str("VHT2SS_MCS0")); + set_env("DEVOURER_TX_NDPA", "1"); /* send_packet marks the NDPA descriptor */ + + /* Self-capture the returned reports on the sounder's RX loop. */ + std::thread snd_rx([&snd, &sensor]() { + snd.dev->StartRxLoop( + [&sensor](const Packet &p) { sensor.feed(p.Data.data(), p.Data.size()); }); + }); + std::thread disp(run_display, std::ref(sensor)); + + /* Build the NDPA once (10-byte rate-less radiotap + 19-byte VHT NDPA body, + * RA = beamformee MAC, TA = canonical SA). Rate is the SetTxMode default. */ + std::vector ndpa = { + 0x00, 0x00, 0x0a, 0x00, 0x00, 0x80, 0x00, 0x00, 0x08, 0x00, /* radiotap */ + 0x54, 0x00, 0x64, 0x00, /* NDPA FC+dur */ + bfe_mac[0], bfe_mac[1], bfe_mac[2], bfe_mac[3], bfe_mac[4], bfe_mac[5], + 0x57, 0x42, 0x75, 0x05, 0xd6, 0x00, /* TA */ + 0x04, 0x00, 0x10 /* dialog token; STA Info: AID0, MU feedback, Nc0 */}; + + logger->info("active: sounding ch{} — move near the setup to see it react", + channel); + /* Sound in a loop, watching the report stream. The beamformee's firmware can + * crash on a long run (and then drop off the USB bus); if reports stop + * advancing, don't spin forever sounding into the void — report it and stop + * cleanly, so a stalled stream can't wedge the adapter. */ + using clk = std::chrono::steady_clock; + long last_total = 0; + auto last_adv = clk::now(); + bool stalled = false; + while (!g_devourer_should_stop) { + if (!snd.dev->send_packet(ndpa.data(), ndpa.size())) + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + long t = sensor.snapshot().total; + auto now = clk::now(); + if (t != last_total) { + last_total = t; + last_adv = now; + } else if (now - last_adv > std::chrono::seconds(kStallSec)) { + logger->warn("report stream stalled {}s — beamformee stopped responding; " + "stopping (unplug/replug it before re-running)", + kStallSec); + stalled = true; + g_devourer_should_stop = true; + } + } + + /* Deadlock-proof shutdown: a wedged USB handle can make an RX-loop join block + * forever. Arm a force-exit safety net so the process is guaranteed to die; + * if the clean joins finish first (the normal case) this timer is killed with + * the process on return and never fires. */ + std::thread([]() { + std::this_thread::sleep_for(std::chrono::seconds(4)); + std::_Exit(0); + }).detach(); + snd.dev->StopRxLoop(); + bfe.dev->StopRxLoop(); + disp.join(); + snd_rx.join(); + bfe_thread.join(); + snd.dev->Stop(); + bfe.dev->Stop(); + return stalled ? 2 : 0; +} + +/* ---------------------------------------------------------------- main ------ */ +static uint16_t hex16(const char *s) { + return (uint16_t)std::strtoul(s, nullptr, 0); +} + +int main(int argc, char **argv) { + auto logger = std::make_shared(); + install_devourer_signal_handlers(); + + int channel = 6; + bool verbose = false; + double sens_k = 4.0; /* CFAR threshold in sigmas; --sensitivity overrides */ + uint16_t snd_vid = 0x0bda, bfe_vid = 0x0bda; + uint16_t snd_pid = 0x8812, bfe_pid = 0xc812; + /* selector: "0xVID:0xPID" or just "0xPID" (VID defaults to 0x0bda). */ + auto parse_sel = [](const std::string &s, uint16_t &v, uint16_t &p) { + auto c = s.find(':'); + if (c != std::string::npos) { + v = hex16(s.substr(0, c).c_str()); + p = hex16(s.substr(c + 1).c_str()); + } else { + p = hex16(s.c_str()); + } + }; + for (int i = 1; i < argc; ++i) { + std::string a = argv[i]; + auto next = [&]() -> std::string { return i + 1 < argc ? argv[++i] : ""; }; + if (a == "--channel") channel = std::atoi(next().c_str()); + else if (a == "--vid") { uint16_t v = hex16(next().c_str()); snd_vid = bfe_vid = v; } + else if (a == "--sounder") parse_sel(next(), snd_vid, snd_pid); + else if (a == "--beamformee") parse_sel(next(), bfe_vid, bfe_pid); + else if (a == "--sensitivity") { + std::string s = next(); + sens_k = s == "high" ? 2.5 : s == "low" ? 6.0 : 4.0; /* default med */ + } + else if (a == "--verbose" || a == "-v") verbose = true; + else if (a == "-h" || a == "--help") { + std::printf( + "WiFiSenseDemo — Wi-Fi motion sensing from beamforming reports\n" + " drives two adapters: a sounder + a beamformee, on one host.\n" + " --channel N (default 6)\n" + " --sensitivity low|med|high detector threshold (default med)\n" + " --vid 0xNNNN default VID for both (default 0x0bda)\n" + " --sounder [VID:]PID sounder adapter selector\n" + " --beamformee [VID:]PID beamformee adapter selector\n" + " -v, --verbose show the library's bring-up logs\n"); + return 0; + } + } + + /* Quiet the library's per-operation info logging so the live display owns the + * console; --verbose restores the full bring-up log. */ + if (!verbose) + logger->set_level(Logger::Level::Warn); + + /* DEVOURER_SENSE_K overrides the detector threshold for on-rig fine-tuning. */ + if (const char *kenv = std::getenv("DEVOURER_SENSE_K")) + sens_k = std::atof(kenv); + + Sensor sensor(sens_k); + return run_active(snd_vid, snd_pid, bfe_vid, bfe_pid, channel, logger, sensor); +} diff --git a/src/BfReportDecode.h b/src/BfReportDecode.h new file mode 100644 index 0000000..7f9e9eb --- /dev/null +++ b/src/BfReportDecode.h @@ -0,0 +1,338 @@ +/* BfReportDecode — C++ decoder for 802.11ac VHT Compressed Beamforming reports. + * + * A direct port of the reference tool `tools/bf_report_decode.py`: it unpacks the + * per-subcarrier Givens angles (phi, psi) out of a captured report frame, and a + * `MotionMeter` turns a stream of reports into a Wi-Fi-sensing signal — the + * per-tone cross-frame variance of psi. A moving person perturbs the channel, so + * that variance rises across the whole band (broadband); a narrowband interferer + * raises it on a few tones (localized). See docs/beamforming-victim-sensing.h. + * + * The compressed V matrix is the right singular vectors of the per-tone channel, + * quantized as Givens rotation angles and packed LSB-first. For the 2-TX / 1-SS + * sounding devourer drives (Nr=2, Nc=1) each subcarrier carries one phi then one + * psi; psi = atan(|h_B|/|h_A|) is the relative per-tone channel between the + * beamformer's two antennas — the quantity that jitters under channel motion. + * + * Header-only, like BfReportDetect.h / BeamformingSounder.h. No devourer runtime + * dependency, so it is unit-testable in isolation (examples/sense/bf_report_decode_selftest.cpp). */ +#ifndef BF_REPORT_DECODE_H +#define BF_REPORT_DECODE_H + +#include +#include +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace devourer::bf { + +/* Subcarriers carried in a report, per bandwidth (0..3 = 20/40/80/160 MHz) and + * grouping Ng (1/2/4). Matches NS_TABLE in the Python tool. 0 = unknown. */ +inline int report_ns(int bw, int ng) { + static const int table[4][3] = {/* Ng=1, 2, 4 */ + {52, 30, 16}, + {108, 58, 30}, + {234, 122, 62}, + {468, 244, 124}}; + int gi = ng == 1 ? 0 : ng == 2 ? 1 : ng == 4 ? 2 : -1; + if (bw < 0 || bw > 3 || gi < 0) + return 0; + return table[bw][gi]; +} + +/* Parsed report header + the location of the V-angle bytes within the frame. */ +struct ReportHdr { + int nc = 0, nr = 0; /* streams / rx antennas (already +1) */ + int bw = 0, ng = 0; /* bandwidth code, grouping */ + bool mu = false; /* feedback: MU (true) / SU (false) */ + bool vht = false; /* VHT (true) / HT (false) */ + int ns = 0; /* subcarriers (report_ns) */ + const uint8_t *angles = nullptr; /* first V-angle byte */ + size_t angle_len = 0; /* V-angle byte count (MU: clamped before the MU-SNR) */ + int per_sc_bits = 0; /* bits per subcarrier across all angles */ +}; + +/* True if `d`(n) is a VHT/HT Compressed Beamforming report; fills `hdr`. Mirrors + * parse_frame() + the MU V-angle slice (ns*10 bits) in the Python tool. */ +inline bool parse_report(const uint8_t *d, size_t n, ReportHdr &hdr) { + if (d == nullptr || n < 30) + return false; + uint8_t sub = d[0] & 0xF0; + if (sub != 0xD0 && sub != 0xE0) /* Action / Action No-Ack */ + return false; + uint8_t cat = d[24], act = d[25]; + bool vht = (cat == 0x15 && act == 0x00); + bool ht = (cat == 0x07 && act == 0x00); + if (!vht && !ht) + return false; + uint32_t mc = (uint32_t)d[26] | ((uint32_t)d[27] << 8) | ((uint32_t)d[28] << 16); + hdr.nc = (int)(mc & 0x7) + 1; + hdr.nr = (int)((mc >> 3) & 0x7) + 1; + hdr.bw = (int)((mc >> 6) & 0x3); + int ng_code = (int)((mc >> 8) & 0x3); + hdr.ng = ng_code == 0 ? 1 : ng_code == 1 ? 2 : 4; + hdr.mu = ((mc >> 11) & 0x1) != 0; + hdr.vht = vht; + hdr.ns = report_ns(hdr.bw, hdr.ng); + if (hdr.ns <= 0 || n < 30 + (size_t)hdr.nc + 4) + return false; + const uint8_t *ab = d + 29 + hdr.nc; /* after per-column avg SNR */ + size_t ab_len = n - (29 + (size_t)hdr.nc) - 4; /* drop 4-byte FCS */ + if (hdr.mu) { + /* MU report: the V-angles are the first ns*10 bits (the Realtek compact 2x1 + * codebook); the MU Exclusive per-tone SNR follows and is not decoded here. */ + hdr.per_sc_bits = 10; + size_t vbytes = ((size_t)hdr.ns * 10 + 7) / 8; + if (vbytes > ab_len) + return false; + ab_len = vbytes; + } else { + size_t bits = ab_len * 8; + if (bits % (size_t)hdr.ns != 0) + return false; + hdr.per_sc_bits = (int)(bits / (size_t)hdr.ns); + } + hdr.angles = ab; + hdr.angle_len = ab_len; + return hdr.per_sc_bits >= 2; +} + +/* LSB-first bit reader over the packed angle stream (802.11 packing order). */ +class BitReader { +public: + BitReader(const uint8_t *data, size_t len) : _d(data), _bits(len * 8) {} + uint32_t read(int n) { + uint32_t v = 0; + for (int i = 0; i < n; ++i) { + if (_pos >= _bits) + break; + uint32_t bit = (_d[_pos >> 3] >> (_pos & 7)) & 1u; + v |= bit << i; + ++_pos; + } + return v; + } + +private: + const uint8_t *_d; + size_t _bits; + size_t _pos = 0; +}; + +inline double dequant_phi(uint32_t q, int b) { + return (2.0 * q + 1.0) * M_PI / (double)(1u << b); +} +inline double dequant_psi(uint32_t q, int b) { + return (2.0 * q + 1.0) * M_PI / (double)(1u << (b + 2)); +} + +/* Number of (phi, psi) angle pairs per subcarrier for the compressed matrix + * (802.11 §19.3.12.3.6). For the 2x1 case this is 1 phi + 1 psi. */ +inline void angle_counts(int nr, int nc, int &nphi, int &npsi) { + nphi = 0; + npsi = 0; + int lim = nc < (nr - 1) ? nc : (nr - 1); + for (int i = 1; i <= lim; ++i) { + for (int r = i; r < nr; ++r) + ++nphi; + for (int r = i; r < nr; ++r) + ++npsi; + } + if (nphi == 0 && npsi == 0) { /* defensive: treat as 2x1 */ + nphi = 1; + npsi = 1; + } +} + +/* Decode the first phi and psi angle of every subcarrier for one report, given a + * bit split. Returns false if the stream is too short. Layout per subcarrier is + * nphi phi's (bphi bits each) then npsi psi's (bpsi bits each). Either output may + * be null. */ +inline bool decode_angles(const ReportHdr &hdr, int bphi, int bpsi, + std::vector *phi_out, + std::vector *psi_out) { + int nphi, npsi; + angle_counts(hdr.nr, hdr.nc, nphi, npsi); + size_t need = (size_t)hdr.ns * (size_t)(nphi * bphi + npsi * bpsi); + if (need > hdr.angle_len * 8) + return false; + BitReader br(hdr.angles, hdr.angle_len); + if (phi_out) + phi_out->assign(hdr.ns, 0.0); + if (psi_out) + psi_out->assign(hdr.ns, 0.0); + for (int k = 0; k < hdr.ns; ++k) { + double first_phi = 0.0, first_psi = 0.0; + for (int p = 0; p < nphi; ++p) { + double phi = dequant_phi(br.read(bphi), bphi); + if (p == 0) + first_phi = phi; + } + for (int p = 0; p < npsi; ++p) { + double psi = dequant_psi(br.read(bpsi), bpsi); + if (p == 0) + first_psi = psi; + } + if (phi_out) + (*phi_out)[k] = first_phi; + if (psi_out) + (*psi_out)[k] = first_psi; + } + return true; +} + +/* Convenience: decode only psi (the amplitude-ratio angle). */ +inline bool decode_psi(const ReportHdr &hdr, int bphi, int bpsi, + std::vector &psi_out) { + return decode_angles(hdr, bphi, bpsi, nullptr, &psi_out); +} + +/* Pick (bphi, bpsi) for a per-subcarrier budget. The 802.11 compressed-Givens + * codebook ALWAYS pairs b_phi = b_psi + 2 (the codebook table is + * (bpsi,bphi) in {(1,3),(2,4),(3,5),(4,6),(5,7),(7,9)}), so we only ever + * consider splits that satisfy it. This is essential, not cosmetic: an + * unconstrained search that minimises cross-frame psi variance is fooled by a + * too-coarse psi — e.g. at (bphi=8,bpsi=2) the 2-bit psi is trivially constant + * (variance 0), so it "wins" the search, but that split is bit-misaligned and + * its phi decodes to garbage that jitters ~30x more on a static channel than + * the correct (6,4) split. Among the (usually one) valid Givens splits, break + * ties by minimum cross-frame psi variance. Returns false if none fits. */ +inline bool pick_split(const std::vector &batch, int &bphi_out, + int &bpsi_out) { + if (batch.empty()) + return false; + int per = batch[0].per_sc_bits, ns = batch[0].ns; + int nphi, npsi; + angle_counts(batch[0].nr, batch[0].nc, nphi, npsi); + double best_var = 1e300; + bool found = false; + for (int bpsi = 1; bpsi <= per; ++bpsi) { + int bphi = bpsi + 2; /* enforce the standard Givens codebook relationship */ + if (nphi * bphi + npsi * bpsi != per) + continue; + /* mean cross-frame variance of psi[k], averaged over tones */ + std::vector sum(ns, 0.0), sumsq(ns, 0.0); + int nfr = 0; + bool ok = true; + for (const auto &h : batch) { + std::vector psi; + if (!decode_psi(h, bphi, bpsi, psi)) { + ok = false; + break; + } + for (int k = 0; k < ns; ++k) { + sum[k] += psi[k]; + sumsq[k] += psi[k] * psi[k]; + } + ++nfr; + } + if (!ok || nfr == 0) + continue; + double var = 0.0; + for (int k = 0; k < ns; ++k) { + double m = sum[k] / nfr; + var += sumsq[k] / nfr - m * m; + } + var /= ns; + if (var < best_var) { + best_var = var; + bphi_out = bphi; + bpsi_out = bpsi; + found = true; + } + } + return found; +} + +/* Sliding-window Wi-Fi-sensing meter: per-tone cross-frame variance of psi over + * the last `window` reports, and the aggregate "motion energy" (mean over tones). + * Fixed split (calibrated once) so the variance is comparable frame to frame. */ +class MotionMeter { +public: + MotionMeter(int ns, int bphi, int bpsi, size_t window = 64) + : _ns(ns), _bphi(bphi), _bpsi(bpsi), _window(window) {} + + /* Decode one report and add its phi[k] to the window. phi (the inter-antenna + * phase, finely quantized) is far more sensitive to motion than psi. Returns + * false if the report's geometry doesn't match this meter (ns) or decode + * failed. */ + bool push(const ReportHdr &hdr) { + if (hdr.ns != _ns) + return false; + std::vector phi; + if (!decode_angles(hdr, _bphi, _bpsi, &phi, nullptr)) + return false; + _buf.push_back(std::move(phi)); + while (_buf.size() > _window) + _buf.pop_front(); + return true; + } + + size_t count() const { return _buf.size(); } + + /* Per-tone circular variance of phi over the window: 1 - |mean(e^{i*phi})|, + * in [0,1]. 0 = the tone's phase is constant (static channel); toward 1 = the + * phase is scattered (the channel is changing frame-to-frame). Circular so it + * handles the 0/2*pi wrap. */ + std::vector per_tone_var() const { + std::vector var(_ns, 0.0); + size_t n = _buf.size(); + if (n < 2) + return var; + for (int k = 0; k < _ns; ++k) { + double cr = 0.0, ci = 0.0; + for (const auto &f : _buf) { + cr += std::cos(f[k]); + ci += std::sin(f[k]); + } + double r = std::sqrt(cr * cr + ci * ci) / n; + var[k] = 1.0 - r; + } + return var; + } + + /* Aggregate motion energy in [0,1]: mean per-tone circular variance. */ + double motion_energy() const { + auto v = per_tone_var(); + if (v.empty()) + return 0.0; + double s = 0.0; + for (double x : v) + s += x; + return s / v.size(); + } + + /* True when the elevated variance is concentrated on a few adjacent tones (a + * narrowband interferer) rather than spread across the band (human motion). + * Heuristic: peak tone variance >> median. */ + bool localized() const { + auto v = per_tone_var(); + if (v.size() < 6) + return false; + std::vector s = v; + std::sort(s.begin(), s.end()); + double median = s[s.size() / 2]; + double peak = s.back(); + /* count tones within 50% of the peak — few = localized, many = broadband */ + double thr = peak * 0.5; + int hot = 0; + for (double x : v) + if (x >= thr) + ++hot; + return median > 1e-9 && peak > 6.0 * median && hot <= (int)v.size() / 6; + } + +private: + int _ns, _bphi, _bpsi; + size_t _window; + std::deque> _buf; +}; + +} // namespace devourer::bf + +#endif /* BF_REPORT_DECODE_H */ diff --git a/src/jaguar1/RadioManagementModule.cpp b/src/jaguar1/RadioManagementModule.cpp index 77e4112..5a27e88 100644 --- a/src/jaguar1/RadioManagementModule.cpp +++ b/src/jaguar1/RadioManagementModule.cpp @@ -435,8 +435,8 @@ void RadioManagementModule::PHY_HandleSwChnlAndSetBW8812( } if (!_setChannelBw && !_swChannel && _needIQK != true) { - _logger->error("[{}]: _swChannel {}, _setChannelBw {}", __func__, - _swChannel, _setChannelBw); + _logger->info("[{}]: _swChannel {}, _setChannelBw {}", __func__, + _swChannel, _setChannelBw); return; } diff --git a/src/logger.h b/src/logger.h index 154af59..cd1d2bc 100644 --- a/src/logger.h +++ b/src/logger.h @@ -131,9 +131,19 @@ using format_string_t = std::string_view; class Logger { public: + /* Verbosity threshold. A message at level L is emitted only when + * _level <= L, so Debug shows everything and Silent shows nothing. Defaults + * to Debug, so existing consumers' output is unchanged; a caller that wants + * a clean stdout (e.g. WiFiSenseDemo's live display) can quiet the library + * with set_level(Logger::Level::Warn). */ + enum class Level { Debug, Info, Warn, Error, Silent }; + void set_level(Level l) { _level = l; } + Level level() const { return _level; } + template void debug(format_string_t fmt, Args &&...args) { #if !defined(NDEBUG) + if (_level > Level::Debug) return; std::string txt = format(fmt, args...); DEVOURER_LOGD(txt.c_str()); #endif @@ -141,21 +151,27 @@ class Logger { template void info(format_string_t fmt, Args &&...args) { + if (_level > Level::Info) return; std::string txt = format(fmt, args...); DEVOURER_LOGI(txt.c_str()); } template void warn(format_string_t fmt, Args &&...args) { + if (_level > Level::Warn) return; std::string txt = format(fmt, args...); DEVOURER_LOGW(txt.c_str()); } template void error(format_string_t fmt, Args &&...args) { + if (_level > Level::Error) return; std::string txt = format(fmt, args...); DEVOURER_LOGE(txt.c_str()); } + +private: + Level _level = Level::Debug; }; using Logger_t = std::shared_ptr;