dearsky/wifi-densepose
1
0
Fork 0
Code Issues 20 Pull Requests 5 Actions Packages Projects Releases 1 Wiki Activity

feat(adr-018): ESP32-S3 firmware, Rust aggregator, and live CSI pipeline

Browse Source 
Complete end-to-end WiFi CSI capture pipeline verified on real hardware:

- ESP32-S3 firmware: WiFi STA + promiscuous mode CSI collection,
  ADR-018 binary serialization, UDP streaming at ~20 Hz
- Rust aggregator CLI binary (clap): receives UDP frames, parses with
  Esp32CsiParser, prints per-frame summary (node, seq, rssi, amp)
- UDP aggregator module with per-node sequence tracking and drop detection
- CsiFrame bridge to detection pipeline (amplitude/phase/SNR conversion)
- Python ESP32 binary parser with UDP reader
- Presence detection confirmed: motion score 10/10 from live CSI variance

Hardware verified: ESP32-S3-DevKitC-1 (CP2102, MAC 3C:0F:02:EC:C2:28),
Docker ESP-IDF v5.2 build, esptool 5.1.0 flash, 20 Rust + 6 Python tests pass.

Co-Authored-By: claude-flow <ruv@ruv.net>
This commit is contained in:
rUv
2026-02-28 13:22:04 -05:00
parent 885627b0a4
commit 92a5182dc3
22 changed files with 1786 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored
View File

@@ -1,3 +1,9 @@
# ESP32 firmware build artifacts and local config (contains WiFi credentials)
firmware/esp32-csi-node/build/
firmware/esp32-csi-node/sdkconfig
firmware/esp32-csi-node/sdkconfig.defaults
firmware/esp32-csi-node/sdkconfig.old
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]

38
README.md
View File

@@ -34,6 +34,44 @@ A cutting-edge WiFi-based human pose estimation system that leverages Channel St
- **WebSocket Streaming**: Real-time pose data streaming for live applications
- **100% Test Coverage**: Thoroughly tested with comprehensive test suite
## ESP32-S3 Hardware Pipeline (ADR-018)
End-to-end WiFi CSI capture verified on real hardware:
```
ESP32-S3 (STA + promiscuous) UDP/5005 Rust aggregator
┌─────────────────────────┐ ──────────> ┌──────────────────┐
│ WiFi CSI callback 20 Hz │ ADR-018 │ Esp32CsiParser │
│ ADR-018 binary frames │ binary │ CsiFrame output │
│ stream_sender (UDP) │ │ presence detect │
└─────────────────────────┘ └──────────────────┘
```
| Metric | Measured |
|--------|----------|
| Frame rate | ~20 Hz sustained |
| Subcarriers | 64 / 128 / 192 (LLTF, HT, HT40) |
| Latency | < 1ms (UDP loopback) |
| Presence detection | Motion score 10/10 at 3m |
**Quick start:**
```bash
# 1. Build firmware (Docker)
cd firmware/esp32-csi-node
docker run --rm -v "$(pwd):/project" -w /project espressif/idf:v5.2 \
bash -c "idf.py set-target esp32s3 && idf.py build"
# 2. Flash to ESP32-S3
python -m esptool --chip esp32s3 --port COM7 --baud 460800 \
write-flash @build/flash_args
# 3. Run aggregator
cargo run -p wifi-densepose-hardware --bin aggregator -- --bind 0.0.0.0:5005
```
See [`firmware/esp32-csi-node/README.md`](firmware/esp32-csi-node/README.md) for detailed setup.
## 🦀 Rust Implementation (v2)
A high-performance Rust port is available in `/rust-port/wifi-densepose-rs/`:

8
firmware/esp32-csi-node/CMakeLists.txt Normal file
View File

@@ -0,0 +1,8 @@
# ESP32 CSI Node Firmware (ADR-018)
# Requires ESP-IDF v5.2+
cmake_minimum_required(VERSION 3.16)
set(EXTRA_COMPONENT_DIRS "")
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(esp32-csi-node)

147
firmware/esp32-csi-node/README.md Normal file
View File

@@ -0,0 +1,147 @@
# ESP32-S3 CSI Node Firmware (ADR-018)
Firmware for ESP32-S3 that collects WiFi Channel State Information (CSI)
and streams it as ADR-018 binary frames over UDP to the aggregator.
Verified working with ESP32-S3-DevKitC-1 (CP2102, MAC 3C:0F:02:EC:C2:28)
streaming ~20 Hz CSI to the Rust aggregator binary.
## Prerequisites
| Component | Version | Purpose |
|-----------|---------|---------|
| Docker Desktop | 28.x+ | Cross-compile ESP-IDF firmware |
| esptool | 5.x+ | Flash firmware to ESP32 |
| ESP32-S3 board | - | Hardware (DevKitC-1 or similar) |
| USB-UART driver | CP210x | Silicon Labs driver for serial |
## Quick Start
### Step 1: Configure WiFi credentials
Create `sdkconfig.defaults` in this directory (it is gitignored):
```
CONFIG_IDF_TARGET="esp32s3"
CONFIG_ESP_WIFI_CSI_ENABLED=y
CONFIG_CSI_NODE_ID=1
CONFIG_CSI_WIFI_SSID="YOUR_WIFI_SSID"
CONFIG_CSI_WIFI_PASSWORD="YOUR_WIFI_PASSWORD"
CONFIG_CSI_TARGET_IP="192.168.1.20"
CONFIG_CSI_TARGET_PORT=5005
CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
```
Replace `YOUR_WIFI_SSID`, `YOUR_WIFI_PASSWORD`, and `CONFIG_CSI_TARGET_IP`
with your actual values. The target IP is the machine running the aggregator.
### Step 2: Build with Docker
```bash
cd firmware/esp32-csi-node
# On Linux/macOS:
docker run --rm -v "$(pwd):/project" -w /project \
espressif/idf:v5.2 bash -c "idf.py set-target esp32s3 && idf.py build"
# On Windows (Git Bash — MSYS path fix required):
MSYS_NO_PATHCONV=1 docker run --rm -v "$(pwd -W)://project" -w //project \
espressif/idf:v5.2 bash -c "idf.py set-target esp32s3 && idf.py build"
```
Build output: `build/bootloader.bin`, `build/partition_table/partition-table.bin`,
`build/esp32-csi-node.bin`.
### Step 3: Flash to ESP32-S3
Find your serial port (`COM7` on Windows, `/dev/ttyUSB0` on Linux):
```bash
cd firmware/esp32-csi-node/build
python -m esptool --chip esp32s3 --port COM7 --baud 460800 \
--before default-reset --after hard-reset \
write-flash --flash-mode dio --flash-freq 80m --flash-size 4MB \
0x0 bootloader/bootloader.bin \
0x8000 partition_table/partition-table.bin \
0x10000 esp32-csi-node.bin
```
### Step 4: Run the aggregator
```bash
cargo run -p wifi-densepose-hardware --bin aggregator -- --bind 0.0.0.0:5005 --verbose
```
Expected output:
```
Listening on 0.0.0.0:5005...
[148 bytes from 192.168.1.71:60764]
[node:1 seq:0] sc=64 rssi=-49 amp=9.5
[276 bytes from 192.168.1.71:60764]
[node:1 seq:1] sc=128 rssi=-64 amp=16.0
```
### Step 5: Verify presence detection
If you see frames streaming (~20/sec), the system is working. Walk near the
ESP32 and observe amplitude variance changes in the CSI data.
## Configuration Reference
Edit via `idf.py menuconfig` or `sdkconfig.defaults`:
| Setting | Default | Description |
|---------|---------|-------------|
| `CSI_NODE_ID` | 1 | Unique node identifier (0-255) |
| `CSI_TARGET_IP` | 192.168.1.100 | Aggregator host IP |
| `CSI_TARGET_PORT` | 5005 | Aggregator UDP port |
| `CSI_WIFI_SSID` | wifi-densepose | WiFi network SSID |
| `CSI_WIFI_PASSWORD` | (empty) | WiFi password |
| `CSI_WIFI_CHANNEL` | 6 | WiFi channel to monitor |
## Firewall Note
On Windows, you may need to allow inbound UDP on port 5005:
```
netsh advfirewall firewall add rule name="ESP32 CSI" dir=in action=allow protocol=UDP localport=5005
```
## Architecture
```
ESP32-S3 Host Machine
+-------------------+ +-------------------+
| WiFi CSI callback | UDP/5005 | aggregator binary |
| (promiscuous mode)| ──────────> | (Rust, clap CLI) |
| ADR-018 serialize | ADR-018 | Esp32CsiParser |
| stream_sender.c | binary frames | CsiFrame output |
+-------------------+ +-------------------+
```
## Binary Frame Format (ADR-018)
```
Offset Size Field
0 4 Magic: 0xC5110001
4 1 Node ID
5 1 Number of antennas
6 2 Number of subcarriers (LE u16)
8 4 Frequency MHz (LE u32)
12 4 Sequence number (LE u32)
16 1 RSSI (i8)
17 1 Noise floor (i8)
18 2 Reserved
20 N*2 I/Q pairs (n_antennas * n_subcarriers * 2 bytes)
```
## Troubleshooting
| Symptom | Cause | Fix |
|---------|-------|-----|
| No serial output | Wrong baud rate | Use 115200 |
| WiFi won't connect | Wrong SSID/password | Check sdkconfig.defaults |
| No UDP frames | Firewall blocking | Add UDP 5005 inbound rule |
| CSI callback not firing | Promiscuous mode off | Verify `esp_wifi_set_promiscuous(true)` in csi_collector.c |
| Parse errors in aggregator | Firmware/parser mismatch | Rebuild both from same source |

4
firmware/esp32-csi-node/main/CMakeLists.txt Normal file
View File

@@ -0,0 +1,4 @@
idf_component_register(
SRCS "main.c" "csi_collector.c" "stream_sender.c"
INCLUDE_DIRS "."
)

42
firmware/esp32-csi-node/main/Kconfig.projbuild Normal file
View File

@@ -0,0 +1,42 @@
menu "CSI Node Configuration"
config CSI_NODE_ID
int "Node ID (0-255)"
default 1
range 0 255
help
Unique identifier for this ESP32 CSI node.
config CSI_TARGET_IP
string "Aggregator IP address"
default "192.168.1.100"
help
IP address of the UDP aggregator host.
config CSI_TARGET_PORT
int "Aggregator UDP port"
default 5005
range 1024 65535
help
UDP port the aggregator listens on.
config CSI_WIFI_SSID
string "WiFi SSID"
default "wifi-densepose"
help
SSID of the WiFi network to connect to.
config CSI_WIFI_PASSWORD
string "WiFi Password"
default ""
help
Password for the WiFi network. Leave empty for open networks.
config CSI_WIFI_CHANNEL
int "WiFi Channel (1-13)"
default 6
range 1 13
help
WiFi channel to listen on for CSI data.
endmenu

176
firmware/esp32-csi-node/main/csi_collector.c Normal file
View File

@@ -0,0 +1,176 @@
/**
* @file csi_collector.c
* @brief CSI data collection and ADR-018 binary frame serialization.
*
* Registers the ESP-IDF WiFi CSI callback and serializes incoming CSI data
* into the ADR-018 binary frame format for UDP transmission.
*/
#include "csi_collector.h"
#include "stream_sender.h"
#include <string.h>
#include "esp_log.h"
#include "esp_wifi.h"
#include "sdkconfig.h"
static const char *TAG = "csi_collector";
static uint32_t s_sequence = 0;
static uint32_t s_cb_count = 0;
static uint32_t s_send_ok = 0;
static uint32_t s_send_fail = 0;
/**
* Serialize CSI data into ADR-018 binary frame format.
*
* Layout:
* [0..3] Magic: 0xC5110001 (LE)
* [4] Node ID
* [5] Number of antennas (rx_ctrl.rx_ant + 1 if available, else 1)
* [6..7] Number of subcarriers (LE u16) = len / (2 * n_antennas)
* [8..11] Frequency MHz (LE u32) — derived from channel
* [12..15] Sequence number (LE u32)
* [16] RSSI (i8)
* [17] Noise floor (i8)
* [18..19] Reserved
* [20..] I/Q data (raw bytes from ESP-IDF callback)
*/
size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf_len)
{
if (info == NULL || buf == NULL || info->buf == NULL) {
return 0;
}
uint8_t n_antennas = 1; /* ESP32-S3 typically reports 1 antenna for CSI */
uint16_t iq_len = (uint16_t)info->len;
uint16_t n_subcarriers = iq_len / (2 * n_antennas);
size_t frame_size = CSI_HEADER_SIZE + iq_len;
if (frame_size > buf_len) {
ESP_LOGW(TAG, "Buffer too small: need %u, have %u", (unsigned)frame_size, (unsigned)buf_len);
return 0;
}
/* Derive frequency from channel number */
uint8_t channel = info->rx_ctrl.channel;
uint32_t freq_mhz;
if (channel >= 1 && channel <= 13) {
freq_mhz = 2412 + (channel - 1) * 5;
} else if (channel == 14) {
freq_mhz = 2484;
} else if (channel >= 36 && channel <= 177) {
freq_mhz = 5000 + channel * 5;
} else {
freq_mhz = 0;
}
/* Magic (LE) */
uint32_t magic = CSI_MAGIC;
memcpy(&buf[0], &magic, 4);
/* Node ID */
buf[4] = (uint8_t)CONFIG_CSI_NODE_ID;
/* Number of antennas */
buf[5] = n_antennas;
/* Number of subcarriers (LE u16) */
memcpy(&buf[6], &n_subcarriers, 2);
/* Frequency MHz (LE u32) */
memcpy(&buf[8], &freq_mhz, 4);
/* Sequence number (LE u32) */
uint32_t seq = s_sequence++;
memcpy(&buf[12], &seq, 4);
/* RSSI (i8) */
buf[16] = (uint8_t)(int8_t)info->rx_ctrl.rssi;
/* Noise floor (i8) */
buf[17] = (uint8_t)(int8_t)info->rx_ctrl.noise_floor;
/* Reserved */
buf[18] = 0;
buf[19] = 0;
/* I/Q data */
memcpy(&buf[CSI_HEADER_SIZE], info->buf, iq_len);
return frame_size;
}
/**
* WiFi CSI callback — invoked by ESP-IDF when CSI data is available.
*/
static void wifi_csi_callback(void *ctx, wifi_csi_info_t *info)
{
(void)ctx;
s_cb_count++;
if (s_cb_count <= 3 || (s_cb_count % 100) == 0) {
ESP_LOGI(TAG, "CSI cb #%lu: len=%d rssi=%d ch=%d",
(unsigned long)s_cb_count, info->len,
info->rx_ctrl.rssi, info->rx_ctrl.channel);
}
uint8_t frame_buf[CSI_MAX_FRAME_SIZE];
size_t frame_len = csi_serialize_frame(info, frame_buf, sizeof(frame_buf));
if (frame_len > 0) {
int ret = stream_sender_send(frame_buf, frame_len);
if (ret > 0) {
s_send_ok++;
} else {
s_send_fail++;
if (s_send_fail <= 5) {
ESP_LOGW(TAG, "sendto failed (fail #%lu)", (unsigned long)s_send_fail);
}
}
}
}
/**
* Promiscuous mode callback — required for CSI to fire on all received frames.
* We don't need the packet content, just the CSI triggered by reception.
*/
static void wifi_promiscuous_cb(void *buf, wifi_promiscuous_pkt_type_t type)
{
/* No-op: CSI callback is registered separately and fires in parallel. */
(void)buf;
(void)type;
}
void csi_collector_init(void)
{
/* Enable promiscuous mode — required for reliable CSI callbacks.
* Without this, CSI only fires on frames destined to this station,
* which may be very infrequent on a quiet network. */
ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true));
ESP_ERROR_CHECK(esp_wifi_set_promiscuous_rx_cb(wifi_promiscuous_cb));
wifi_promiscuous_filter_t filt = {
.filter_mask = WIFI_PROMIS_FILTER_MASK_MGMT | WIFI_PROMIS_FILTER_MASK_DATA,
};
ESP_ERROR_CHECK(esp_wifi_set_promiscuous_filter(&filt));
ESP_LOGI(TAG, "Promiscuous mode enabled for CSI capture");
wifi_csi_config_t csi_config = {
.lltf_en = true,
.htltf_en = true,
.stbc_htltf2_en = true,
.ltf_merge_en = true,
.channel_filter_en = false,
.manu_scale = false,
.shift = false,
};
ESP_ERROR_CHECK(esp_wifi_set_csi_config(&csi_config));
ESP_ERROR_CHECK(esp_wifi_set_csi_rx_cb(wifi_csi_callback, NULL));
ESP_ERROR_CHECK(esp_wifi_set_csi(true));
ESP_LOGI(TAG, "CSI collection initialized (node_id=%d, channel=%d)",
CONFIG_CSI_NODE_ID, CONFIG_CSI_WIFI_CHANNEL);
}

38
firmware/esp32-csi-node/main/csi_collector.h Normal file
View File

@@ -0,0 +1,38 @@
/**
* @file csi_collector.h
* @brief CSI data collection and ADR-018 binary frame serialization.
*/
#ifndef CSI_COLLECTOR_H
#define CSI_COLLECTOR_H
#include <stdint.h>
#include <stddef.h>
#include "esp_wifi_types.h"
/** ADR-018 magic number. */
#define CSI_MAGIC 0xC5110001
/** ADR-018 header size in bytes. */
#define CSI_HEADER_SIZE 20
/** Maximum frame buffer size (header + 4 antennas * 256 subcarriers * 2 bytes). */
#define CSI_MAX_FRAME_SIZE (CSI_HEADER_SIZE + 4 * 256 * 2)
/**
* Initialize CSI collection.
* Registers the WiFi CSI callback.
*/
void csi_collector_init(void);
/**
* Serialize CSI data into ADR-018 binary frame format.
*
* @param info WiFi CSI info from the ESP-IDF callback.
* @param buf Output buffer (must be at least CSI_MAX_FRAME_SIZE bytes).
* @param buf_len Size of the output buffer.
* @return Number of bytes written, or 0 on error.
*/
size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf_len);
#endif /* CSI_COLLECTOR_H */

137
firmware/esp32-csi-node/main/main.c Normal file
View File

@@ -0,0 +1,137 @@
/**
* @file main.c
* @brief ESP32-S3 CSI Node — ADR-018 compliant firmware.
*
* Initializes NVS, WiFi STA mode, CSI collection, and UDP streaming.
* CSI frames are serialized in ADR-018 binary format and sent to the
* aggregator over UDP.
*/
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "sdkconfig.h"
#include "csi_collector.h"
#include "stream_sender.h"
static const char *TAG = "main";
/* Event group bits */
#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT BIT1
static EventGroupHandle_t s_wifi_event_group;
static int s_retry_num = 0;
#define MAX_RETRY 10
static void event_handler(void *arg, esp_event_base_t event_base,
int32_t event_id, void *event_data)
{
if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
esp_wifi_connect();
} else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
if (s_retry_num < MAX_RETRY) {
esp_wifi_connect();
s_retry_num++;
ESP_LOGI(TAG, "Retrying WiFi connection (%d/%d)", s_retry_num, MAX_RETRY);
} else {
xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
}
} else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));
s_retry_num = 0;
xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
}
}
static void wifi_init_sta(void)
{
s_wifi_event_group = xEventGroupCreate();
ESP_ERROR_CHECK(esp_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default());
esp_netif_create_default_wifi_sta();
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
esp_event_handler_instance_t instance_any_id;
esp_event_handler_instance_t instance_got_ip;
ESP_ERROR_CHECK(esp_event_handler_instance_register(
WIFI_EVENT, ESP_EVENT_ANY_ID, &event_handler, NULL, &instance_any_id));
ESP_ERROR_CHECK(esp_event_handler_instance_register(
IP_EVENT, IP_EVENT_STA_GOT_IP, &event_handler, NULL, &instance_got_ip));
wifi_config_t wifi_config = {
.sta = {
.ssid = CONFIG_CSI_WIFI_SSID,
#ifdef CONFIG_CSI_WIFI_PASSWORD
.password = CONFIG_CSI_WIFI_PASSWORD,
#endif
.threshold.authmode = WIFI_AUTH_WPA2_PSK,
},
};
/* If password is empty, use open auth */
if (strlen((char *)wifi_config.sta.password) == 0) {
wifi_config.sta.threshold.authmode = WIFI_AUTH_OPEN;
}
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_LOGI(TAG, "WiFi STA initialized, connecting to SSID: %s", CONFIG_CSI_WIFI_SSID);
/* Wait for connection */
EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
pdFALSE, pdFALSE, portMAX_DELAY);
if (bits & WIFI_CONNECTED_BIT) {
ESP_LOGI(TAG, "Connected to WiFi");
} else if (bits & WIFI_FAIL_BIT) {
ESP_LOGE(TAG, "Failed to connect to WiFi after %d retries", MAX_RETRY);
}
}
void app_main(void)
{
ESP_LOGI(TAG, "ESP32-S3 CSI Node (ADR-018) — Node ID: %d", CONFIG_CSI_NODE_ID);
/* Initialize NVS */
esp_err_t ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
ret = nvs_flash_init();
}
ESP_ERROR_CHECK(ret);
/* Initialize WiFi STA */
wifi_init_sta();
/* Initialize UDP sender */
if (stream_sender_init() != 0) {
ESP_LOGE(TAG, "Failed to initialize UDP sender");
return;
}
/* Initialize CSI collection */
csi_collector_init();
ESP_LOGI(TAG, "CSI streaming active → %s:%d",
CONFIG_CSI_TARGET_IP, CONFIG_CSI_TARGET_PORT);
/* Main loop — keep alive */
while (1) {
vTaskDelay(pdMS_TO_TICKS(10000));
}
}

67
firmware/esp32-csi-node/main/stream_sender.c Normal file
View File

@@ -0,0 +1,67 @@
/**
* @file stream_sender.c
* @brief UDP stream sender for CSI frames.
*
* Opens a UDP socket and sends serialized ADR-018 frames to the aggregator.
*/
#include "stream_sender.h"
#include <string.h>
#include "esp_log.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"
#include "sdkconfig.h"
static const char *TAG = "stream_sender";
static int s_sock = -1;
static struct sockaddr_in s_dest_addr;
int stream_sender_init(void)
{
s_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (s_sock < 0) {
ESP_LOGE(TAG, "Failed to create socket: errno %d", errno);
return -1;
}
memset(&s_dest_addr, 0, sizeof(s_dest_addr));
s_dest_addr.sin_family = AF_INET;
s_dest_addr.sin_port = htons(CONFIG_CSI_TARGET_PORT);
if (inet_pton(AF_INET, CONFIG_CSI_TARGET_IP, &s_dest_addr.sin_addr) <= 0) {
ESP_LOGE(TAG, "Invalid target IP: %s", CONFIG_CSI_TARGET_IP);
close(s_sock);
s_sock = -1;
return -1;
}
ESP_LOGI(TAG, "UDP sender initialized: %s:%d", CONFIG_CSI_TARGET_IP, CONFIG_CSI_TARGET_PORT);
return 0;
}
int stream_sender_send(const uint8_t *data, size_t len)
{
if (s_sock < 0) {
return -1;
}
int sent = sendto(s_sock, data, len, 0,
(struct sockaddr *)&s_dest_addr, sizeof(s_dest_addr));
if (sent < 0) {
ESP_LOGW(TAG, "sendto failed: errno %d", errno);
return -1;
}
return sent;
}
void stream_sender_deinit(void)
{
if (s_sock >= 0) {
close(s_sock);
s_sock = -1;
ESP_LOGI(TAG, "UDP sender closed");
}
}

34
firmware/esp32-csi-node/main/stream_sender.h Normal file
View File

@@ -0,0 +1,34 @@
/**
* @file stream_sender.h
* @brief UDP stream sender for CSI frames.
*/
#ifndef STREAM_SENDER_H
#define STREAM_SENDER_H
#include <stdint.h>
#include <stddef.h>
/**
* Initialize the UDP sender.
* Creates a UDP socket targeting the configured aggregator.
*
* @return 0 on success, -1 on error.
*/
int stream_sender_init(void);
/**
* Send a serialized CSI frame over UDP.
*
* @param data Frame data buffer.
* @param len Length of data to send.
* @return Number of bytes sent, or -1 on error.
*/
int stream_sender_send(const uint8_t *data, size_t len);
/**
* Close the UDP sender socket.
*/
void stream_sender_deinit(void);
#endif /* STREAM_SENDER_H */

1
rust-port/wifi-densepose-rs/Cargo.lock generated
View File

@@ -3966,6 +3966,7 @@ dependencies = [
"approx",
"byteorder",
"chrono",
"clap",
"serde",
"serde_json",
"thiserror 1.0.69",

2
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/Cargo.toml
View File

@@ -17,6 +17,8 @@ intel5300 = []
linux-wifi = []
[dependencies]
# CLI argument parsing (for bin/aggregator)
clap = { version = "4.4", features = ["derive"] }
# Byte parsing
byteorder = "1.5"
# Time

276
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/aggregator/mod.rs Normal file
View File

@@ -0,0 +1,276 @@
//! UDP aggregator for ESP32 CSI nodes (ADR-018 Layer 2).
//!
//! Receives ADR-018 binary frames over UDP from multiple ESP32 nodes,
//! parses them, tracks per-node state (sequence gaps, drop counting),
//! and forwards parsed `CsiFrame`s to the processing pipeline via an
//! `mpsc` channel.
use std::collections::HashMap;
use std::io;
use std::net::{SocketAddr, UdpSocket};
use std::sync::mpsc::{self, SyncSender, Receiver};
use crate::csi_frame::CsiFrame;
use crate::esp32_parser::Esp32CsiParser;
/// Configuration for the UDP aggregator.
#[derive(Debug, Clone)]
pub struct AggregatorConfig {
/// Address to bind the UDP socket to.
pub bind_addr: String,
/// Port to listen on.
pub port: u16,
/// Channel capacity for the frame sender (0 = unbounded-like behavior via sync).
pub channel_capacity: usize,
}
impl Default for AggregatorConfig {
fn default() -> Self {
Self {
bind_addr: "0.0.0.0".to_string(),
port: 5005,
channel_capacity: 1024,
}
}
}
/// Per-node tracking state.
#[derive(Debug)]
struct NodeState {
/// Last seen sequence number.
last_sequence: u32,
/// Total frames received from this node.
frames_received: u64,
/// Total dropped frames detected (sequence gaps).
frames_dropped: u64,
}
impl NodeState {
fn new(initial_sequence: u32) -> Self {
Self {
last_sequence: initial_sequence,
frames_received: 1,
frames_dropped: 0,
}
}
/// Update state with a new sequence number. Returns the gap size (0 if contiguous).
fn update(&mut self, sequence: u32) -> u32 {
self.frames_received += 1;
let expected = self.last_sequence.wrapping_add(1);
let gap = if sequence > expected {
sequence - expected
} else {
0
};
self.frames_dropped += gap as u64;
self.last_sequence = sequence;
gap
}
}
/// UDP aggregator that receives CSI frames from ESP32 nodes.
pub struct Esp32Aggregator {
socket: UdpSocket,
nodes: HashMap<u8, NodeState>,
tx: SyncSender<CsiFrame>,
}
impl Esp32Aggregator {
/// Create a new aggregator bound to the configured address.
pub fn new(config: &AggregatorConfig) -> io::Result<(Self, Receiver<CsiFrame>)> {
let addr: SocketAddr = format!("{}:{}", config.bind_addr, config.port)
.parse()
.map_err(|e| io::Error::new(io::ErrorKind::InvalidInput, e))?;
let socket = UdpSocket::bind(addr)?;
let (tx, rx) = mpsc::sync_channel(config.channel_capacity);
Ok((
Self {
socket,
nodes: HashMap::new(),
tx,
},
rx,
))
}
/// Create an aggregator from an existing socket (for testing).
pub fn from_socket(socket: UdpSocket, tx: SyncSender<CsiFrame>) -> Self {
Self {
socket,
nodes: HashMap::new(),
tx,
}
}
/// Run the blocking receive loop. Call from a dedicated thread.
pub fn run(&mut self) -> io::Result<()> {
let mut buf = [0u8; 2048];
loop {
let (n, _src) = self.socket.recv_from(&mut buf)?;
self.handle_packet(&buf[..n]);
}
}
/// Handle a single UDP packet. Public for unit testing.
pub fn handle_packet(&mut self, data: &[u8]) {
match Esp32CsiParser::parse_frame(data) {
Ok((frame, _consumed)) => {
let node_id = frame.metadata.node_id;
let seq = frame.metadata.sequence;
// Track node state
match self.nodes.get_mut(&node_id) {
Some(state) => {
state.update(seq);
}
None => {
self.nodes.insert(node_id, NodeState::new(seq));
}
}
// Send to channel (ignore send errors — receiver may have dropped)
let _ = self.tx.try_send(frame);
}
Err(_) => {
// Bad packet — silently drop (per ADR-018: aggregator is tolerant)
}
}
}
/// Get the number of dropped frames for a specific node.
pub fn drops_for_node(&self, node_id: u8) -> u64 {
self.nodes.get(&node_id).map_or(0, |s| s.frames_dropped)
}
/// Get the number of tracked nodes.
pub fn node_count(&self) -> usize {
self.nodes.len()
}
}
#[cfg(test)]
mod tests {
use super::*;
use std::sync::mpsc;
/// Helper: build an ADR-018 frame packet for testing.
fn build_test_packet(node_id: u8, sequence: u32, n_subcarriers: usize) -> Vec<u8> {
let mut buf = Vec::new();
// Magic
buf.extend_from_slice(&0xC5110001u32.to_le_bytes());
// Node ID
buf.push(node_id);
// Antennas
buf.push(1);
// Subcarriers (LE u16)
buf.extend_from_slice(&(n_subcarriers as u16).to_le_bytes());
// Frequency MHz (LE u32)
buf.extend_from_slice(&2437u32.to_le_bytes());
// Sequence (LE u32)
buf.extend_from_slice(&sequence.to_le_bytes());
// RSSI (i8)
buf.push((-50i8) as u8);
// Noise floor (i8)
buf.push((-90i8) as u8);
// Reserved
buf.extend_from_slice(&[0u8; 2]);
// I/Q data
for i in 0..n_subcarriers {
buf.push((i % 127) as u8); // I
buf.push(((i * 2) % 127) as u8); // Q
}
buf
}
#[test]
fn test_aggregator_receives_valid_frame() {
let (tx, rx) = mpsc::sync_channel(16);
let socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let mut agg = Esp32Aggregator::from_socket(socket, tx);
let pkt = build_test_packet(1, 0, 4);
agg.handle_packet(&pkt);
let frame = rx.try_recv().unwrap();
assert_eq!(frame.metadata.node_id, 1);
assert_eq!(frame.metadata.sequence, 0);
assert_eq!(frame.subcarrier_count(), 4);
}
#[test]
fn test_aggregator_tracks_sequence_gaps() {
let (tx, _rx) = mpsc::sync_channel(16);
let socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let mut agg = Esp32Aggregator::from_socket(socket, tx);
// Send seq 0
agg.handle_packet(&build_test_packet(1, 0, 4));
// Send seq 5 (gap of 4)
agg.handle_packet(&build_test_packet(1, 5, 4));
assert_eq!(agg.drops_for_node(1), 4);
}
#[test]
fn test_aggregator_handles_bad_packet() {
let (tx, rx) = mpsc::sync_channel(16);
let socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let mut agg = Esp32Aggregator::from_socket(socket, tx);
// Garbage bytes — should not panic or produce a frame
agg.handle_packet(&[0xFF, 0xFE, 0xFD, 0xFC, 0x00]);
assert!(rx.try_recv().is_err());
assert_eq!(agg.node_count(), 0);
}
#[test]
fn test_aggregator_multi_node() {
let (tx, rx) = mpsc::sync_channel(16);
let socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let mut agg = Esp32Aggregator::from_socket(socket, tx);
agg.handle_packet(&build_test_packet(1, 0, 4));
agg.handle_packet(&build_test_packet(2, 0, 4));
assert_eq!(agg.node_count(), 2);
let f1 = rx.try_recv().unwrap();
let f2 = rx.try_recv().unwrap();
assert_eq!(f1.metadata.node_id, 1);
assert_eq!(f2.metadata.node_id, 2);
}
#[test]
fn test_aggregator_loopback_udp() {
// Full UDP roundtrip via loopback
let recv_socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let recv_addr = recv_socket.local_addr().unwrap();
recv_socket.set_nonblocking(true).unwrap();
let send_socket = UdpSocket::bind("127.0.0.1:0").unwrap();
let (tx, rx) = mpsc::sync_channel(16);
let mut agg = Esp32Aggregator::from_socket(recv_socket, tx);
// Send a packet via UDP
let pkt = build_test_packet(3, 42, 4);
send_socket.send_to(&pkt, recv_addr).unwrap();
// Read from the socket and handle
let mut buf = [0u8; 2048];
// Small delay to let the packet arrive
std::thread::sleep(std::time::Duration::from_millis(50));
if let Ok((n, _)) = agg.socket.recv_from(&mut buf) {
agg.handle_packet(&buf[..n]);
}
let frame = rx.try_recv().unwrap();
assert_eq!(frame.metadata.node_id, 3);
assert_eq!(frame.metadata.sequence, 42);
}
}

75
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/bin/aggregator.rs Normal file
View File

@@ -0,0 +1,75 @@
//! UDP aggregator CLI for receiving ESP32 CSI frames (ADR-018).
//!
//! Listens for ADR-018 binary CSI frames on a UDP socket, parses each
//! packet, and prints a one-line summary to stdout.
//!
//! Usage:
//! cargo run -p wifi-densepose-hardware --bin aggregator -- --bind 0.0.0.0:5005
use std::net::UdpSocket;
use std::process;
use clap::Parser;
use wifi_densepose_hardware::Esp32CsiParser;
/// UDP aggregator for ESP32 CSI nodes (ADR-018).
#[derive(Parser)]
#[command(name = "aggregator", about = "Receive and display live CSI frames from ESP32 nodes")]
struct Cli {
/// Address:port to bind the UDP listener to.
#[arg(long, default_value = "0.0.0.0:5005")]
bind: String,
/// Print raw hex dump alongside parsed output.
#[arg(long, short)]
verbose: bool,
}
fn main() {
let cli = Cli::parse();
let socket = match UdpSocket::bind(&cli.bind) {
Ok(s) => s,
Err(e) => {
eprintln!("Error: cannot bind to {}: {}", cli.bind, e);
process::exit(1);
}
};
eprintln!("Listening on {}...", cli.bind);
let mut buf = [0u8; 2048];
loop {
let (n, src) = match socket.recv_from(&mut buf) {
Ok(r) => r,
Err(e) => {
eprintln!("recv error: {}", e);
continue;
}
};
if cli.verbose {
eprintln!(" [{} bytes from {}]", n, src);
}
match Esp32CsiParser::parse_frame(&buf[..n]) {
Ok((frame, _consumed)) => {
let mean_amp = frame.mean_amplitude();
println!(
"[node:{} seq:{}] sc={} rssi={} amp={:.1}",
frame.metadata.node_id,
frame.metadata.sequence,
frame.subcarrier_count(),
frame.metadata.rssi_dbm,
mean_amp,
);
}
Err(e) => {
if cli.verbose {
eprintln!(" parse error: {}", e);
}
}
}
}
}

169
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/bridge.rs Normal file
View File

@@ -0,0 +1,169 @@
//! CsiFrame → CsiData bridge (ADR-018 Layer 3).
//!
//! Converts hardware-level `CsiFrame` (I/Q pairs) into the pipeline-ready
//! `CsiData` format (amplitude/phase vectors). No ndarray dependency —
//! uses plain `Vec<f64>`.
use crate::csi_frame::CsiFrame;
/// Pipeline-ready CSI data with amplitude and phase vectors (ADR-018).
#[derive(Debug, Clone)]
pub struct CsiData {
/// Unix timestamp in milliseconds when the frame was received.
pub timestamp_unix_ms: u64,
/// Node identifier (0-255).
pub node_id: u8,
/// Number of antennas.
pub n_antennas: usize,
/// Number of subcarriers per antenna.
pub n_subcarriers: usize,
/// Amplitude values: sqrt(I² + Q²) for each (antenna, subcarrier).
/// Length = n_antennas * n_subcarriers, laid out antenna-major.
pub amplitude: Vec<f64>,
/// Phase values: atan2(Q, I) for each (antenna, subcarrier).
/// Length = n_antennas * n_subcarriers.
pub phase: Vec<f64>,
/// RSSI in dBm.
pub rssi_dbm: i8,
/// Noise floor in dBm.
pub noise_floor_dbm: i8,
/// Channel center frequency in MHz.
pub channel_freq_mhz: u32,
/// Sequence number.
pub sequence: u32,
}
impl CsiData {
/// Compute SNR as RSSI - noise floor (in dB).
pub fn snr_db(&self) -> f64 {
self.rssi_dbm as f64 - self.noise_floor_dbm as f64
}
}
impl From<CsiFrame> for CsiData {
fn from(frame: CsiFrame) -> Self {
let n_antennas = frame.metadata.n_antennas as usize;
let n_subcarriers = frame.metadata.n_subcarriers as usize;
let total = frame.subcarriers.len();
let mut amplitude = Vec::with_capacity(total);
let mut phase = Vec::with_capacity(total);
for sc in &frame.subcarriers {
let i = sc.i as f64;
let q = sc.q as f64;
amplitude.push((i * i + q * q).sqrt());
phase.push(q.atan2(i));
}
let timestamp_unix_ms = frame.metadata.timestamp.timestamp_millis() as u64;
CsiData {
timestamp_unix_ms,
node_id: frame.metadata.node_id,
n_antennas,
n_subcarriers,
amplitude,
phase,
rssi_dbm: frame.metadata.rssi_dbm,
noise_floor_dbm: frame.metadata.noise_floor_dbm,
channel_freq_mhz: frame.metadata.channel_freq_mhz,
sequence: frame.metadata.sequence,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::csi_frame::{AntennaConfig, Bandwidth, CsiMetadata, SubcarrierData};
use chrono::Utc;
fn make_frame(
node_id: u8,
n_antennas: u8,
subcarriers: Vec<SubcarrierData>,
) -> CsiFrame {
let n_subcarriers = if n_antennas == 0 {
subcarriers.len()
} else {
subcarriers.len() / n_antennas as usize
};
CsiFrame {
metadata: CsiMetadata {
timestamp: Utc::now(),
node_id,
n_antennas,
n_subcarriers: n_subcarriers as u16,
channel_freq_mhz: 2437,
rssi_dbm: -45,
noise_floor_dbm: -90,
bandwidth: Bandwidth::Bw20,
antenna_config: AntennaConfig {
tx_antennas: 1,
rx_antennas: n_antennas,
},
sequence: 42,
},
subcarriers,
}
}
#[test]
fn test_bridge_from_known_iq() {
let subs = vec![
SubcarrierData { i: 3, q: 4, index: -1 }, // amp = 5.0
SubcarrierData { i: 0, q: 10, index: 1 }, // amp = 10.0
];
let frame = make_frame(1, 1, subs);
let data: CsiData = frame.into();
assert_eq!(data.amplitude.len(), 2);
assert!((data.amplitude[0] - 5.0).abs() < 0.001);
assert!((data.amplitude[1] - 10.0).abs() < 0.001);
}
#[test]
fn test_bridge_multi_antenna() {
// 2 antennas, 3 subcarriers each = 6 total
let subs = vec![
SubcarrierData { i: 1, q: 0, index: -1 },
SubcarrierData { i: 2, q: 0, index: 0 },
SubcarrierData { i: 3, q: 0, index: 1 },
SubcarrierData { i: 4, q: 0, index: -1 },
SubcarrierData { i: 5, q: 0, index: 0 },
SubcarrierData { i: 6, q: 0, index: 1 },
];
let frame = make_frame(1, 2, subs);
let data: CsiData = frame.into();
assert_eq!(data.n_antennas, 2);
assert_eq!(data.n_subcarriers, 3);
assert_eq!(data.amplitude.len(), 6);
assert_eq!(data.phase.len(), 6);
}
#[test]
fn test_bridge_snr_computation() {
let subs = vec![SubcarrierData { i: 1, q: 0, index: 0 }];
let frame = make_frame(1, 1, subs);
let data: CsiData = frame.into();
// rssi=-45, noise=-90, SNR=45
assert!((data.snr_db() - 45.0).abs() < 0.001);
}
#[test]
fn test_bridge_preserves_metadata() {
let subs = vec![SubcarrierData { i: 10, q: 20, index: 0 }];
let frame = make_frame(7, 1, subs);
let data: CsiData = frame.into();
assert_eq!(data.node_id, 7);
assert_eq!(data.channel_freq_mhz, 2437);
assert_eq!(data.sequence, 42);
assert_eq!(data.rssi_dbm, -45);
assert_eq!(data.noise_floor_dbm, -90);
}
}

39
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/csi_frame.rs
View File

@@ -57,25 +57,27 @@ impl CsiFrame {
}
}
/// Metadata associated with a CSI frame.
/// Metadata associated with a CSI frame (ADR-018 format).
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct CsiMetadata {
/// Timestamp when frame was received
pub timestamp: DateTime<Utc>,
/// RSSI in dBm (typically -100 to 0)
pub rssi: i32,
/// Noise floor in dBm
pub noise_floor: i32,
/// WiFi channel number
pub channel: u8,
/// Secondary channel offset (0, 1, or 2)
pub secondary_channel: u8,
/// Channel bandwidth
/// Node identifier (0-255)
pub node_id: u8,
/// Number of antennas
pub n_antennas: u8,
/// Number of subcarriers
pub n_subcarriers: u16,
/// Channel center frequency in MHz
pub channel_freq_mhz: u32,
/// RSSI in dBm (signed byte, typically -100 to 0)
pub rssi_dbm: i8,
/// Noise floor in dBm (signed byte)
pub noise_floor_dbm: i8,
/// Channel bandwidth (derived from n_subcarriers)
pub bandwidth: Bandwidth,
/// Antenna configuration
/// Antenna configuration (populated from n_antennas)
pub antenna_config: AntennaConfig,
/// Source MAC address (if available)
pub source_mac: Option<[u8; 6]>,
/// Sequence number for ordering
pub sequence: u32,
}
@@ -143,13 +145,14 @@ mod tests {
CsiFrame {
metadata: CsiMetadata {
timestamp: Utc::now(),
rssi: -50,
noise_floor: -95,
channel: 6,
secondary_channel: 0,
node_id: 1,
n_antennas: 1,
n_subcarriers: 3,
channel_freq_mhz: 2437,
rssi_dbm: -50,
noise_floor_dbm: -95,
bandwidth: Bandwidth::Bw20,
antenna_config: AntennaConfig::default(),
source_mac: None,
sequence: 1,
},
subcarriers: vec![

6
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/error.rs
View File

@@ -39,6 +39,12 @@ pub enum ParseError {
value: i32,
},
/// Invalid antenna count (must be 1-4 for ESP32).
#[error("Invalid antenna count: {count} (expected 1-4)")]
InvalidAntennaCount {
count: u8,
},
/// Generic byte-level parse error.
#[error("Parse error at offset {offset}: {message}")]
ByteError {

302
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/esp32_parser.rs
View File

@@ -1,28 +1,26 @@
//! ESP32 CSI frame parser.
//! ESP32 CSI frame parser (ADR-018 binary format).
//!
//! Parses binary CSI data as produced by ESP-IDF's `wifi_csi_info_t` structure,
//! typically streamed over serial (UART at 921600 baud) or UDP.
//! Parses binary CSI data as produced by ADR-018 compliant firmware,
//! typically streamed over UDP from ESP32/ESP32-S3 nodes.
//!
//! # ESP32 CSI Binary Format
//!
//! The ESP32 CSI callback produces a buffer with the following layout:
//! # ADR-018 Binary Frame Format
//!
//! ```text
//! Offset Size Field
//! ------ ---- -----
//! 0 4 Magic (0xCSI10001 or as configured in firmware)
//! 4 4 Sequence number
//! 8 1 Channel
//! 9 1 Secondary channel
//! 10 1 RSSI (signed)
//! 11 1 Noise floor (signed)
//! 12 2 CSI data length (number of I/Q bytes)
//! 14 6 Source MAC address
//! 20 N I/Q data (pairs of i8 values, 2 bytes per subcarrier)
//! 0 4 Magic: 0xC5110001
//! 4 1 Node ID
//! 5 1 Number of antennas
//! 6 2 Number of subcarriers (LE u16)
//! 8 4 Frequency MHz (LE u32)
//! 12 4 Sequence number (LE u32)
//! 16 1 RSSI (i8)
//! 17 1 Noise floor (i8)
//! 18 2 Reserved
//! 20 N*2 I/Q pairs (n_antennas * n_subcarriers * 2 bytes)
//! ```
//!
//! Each subcarrier contributes 2 bytes: one signed byte for I, one for Q.
//! For 20 MHz bandwidth with 56 subcarriers: N = 112 bytes.
//! Each I/Q pair is 2 signed bytes: I then Q.
//!
//! # No-Mock Guarantee
//!
@@ -36,17 +34,19 @@ use std::io::Cursor;
use crate::csi_frame::{AntennaConfig, Bandwidth, CsiFrame, CsiMetadata, SubcarrierData};
use crate::error::ParseError;
/// ESP32 CSI binary frame magic number.
///
/// This is a convention for the firmware framing protocol.
/// The actual ESP-IDF callback doesn't include a magic number;
/// our recommended firmware adds this for reliable frame sync.
/// ESP32 CSI binary frame magic number (ADR-018).
const ESP32_CSI_MAGIC: u32 = 0xC5110001;
/// Maximum valid subcarrier count for ESP32 (80MHz bandwidth).
/// ADR-018 header size in bytes (before I/Q data).
const HEADER_SIZE: usize = 20;
/// Maximum valid subcarrier count for ESP32 (80 MHz bandwidth).
const MAX_SUBCARRIERS: usize = 256;
/// Parser for ESP32 CSI binary frames.
/// Maximum antenna count for ESP32.
const MAX_ANTENNAS: u8 = 4;
/// Parser for ESP32 CSI binary frames (ADR-018 format).
pub struct Esp32CsiParser;
impl Esp32CsiParser {
@@ -55,16 +55,16 @@ impl Esp32CsiParser {
/// The buffer must contain at least the header (20 bytes) plus the I/Q data.
/// Returns the parsed frame and the number of bytes consumed.
pub fn parse_frame(data: &[u8]) -> Result<(CsiFrame, usize), ParseError> {
if data.len() < 20 {
if data.len() < HEADER_SIZE {
return Err(ParseError::InsufficientData {
needed: 20,
needed: HEADER_SIZE,
got: data.len(),
});
}
let mut cursor = Cursor::new(data);
// Read magic
// Magic (offset 0, 4 bytes)
let magic = cursor.read_u32::<LittleEndian>().map_err(|_| ParseError::InsufficientData {
needed: 4,
got: 0,
@@ -77,72 +77,70 @@ impl Esp32CsiParser {
});
}
// Sequence number
let sequence = cursor.read_u32::<LittleEndian>().map_err(|_| ParseError::InsufficientData {
needed: 8,
got: 4,
// Node ID (offset 4, 1 byte)
let node_id = cursor.read_u8().map_err(|_| ParseError::ByteError {
offset: 4,
message: "Failed to read node ID".into(),
})?;
// Channel info
let channel = cursor.read_u8().map_err(|_| ParseError::ByteError {
offset: 8,
message: "Failed to read channel".into(),
// Number of antennas (offset 5, 1 byte)
let n_antennas = cursor.read_u8().map_err(|_| ParseError::ByteError {
offset: 5,
message: "Failed to read antenna count".into(),
})?;
let secondary_channel = cursor.read_u8().map_err(|_| ParseError::ByteError {
offset: 9,
message: "Failed to read secondary channel".into(),
})?;
// RSSI (signed)
let rssi = cursor.read_i8().map_err(|_| ParseError::ByteError {
offset: 10,
message: "Failed to read RSSI".into(),
})? as i32;
if rssi > 0 || rssi < -100 {
return Err(ParseError::InvalidRssi { value: rssi });
if n_antennas == 0 || n_antennas > MAX_ANTENNAS {
return Err(ParseError::InvalidAntennaCount { count: n_antennas });
}
// Noise floor (signed)
let noise_floor = cursor.read_i8().map_err(|_| ParseError::ByteError {
offset: 11,
message: "Failed to read noise floor".into(),
})? as i32;
// CSI data length
let iq_length = cursor.read_u16::<LittleEndian>().map_err(|_| ParseError::ByteError {
offset: 12,
message: "Failed to read I/Q length".into(),
// Number of subcarriers (offset 6, 2 bytes LE)
let n_subcarriers = cursor.read_u16::<LittleEndian>().map_err(|_| ParseError::ByteError {
offset: 6,
message: "Failed to read subcarrier count".into(),
})? as usize;
// Source MAC
let mut mac = [0u8; 6];
for (i, byte) in mac.iter_mut().enumerate() {
*byte = cursor.read_u8().map_err(|_| ParseError::ByteError {
offset: 14 + i,
message: "Failed to read MAC address".into(),
})?;
}
// Validate I/Q length
let subcarrier_count = iq_length / 2;
if subcarrier_count > MAX_SUBCARRIERS {
if n_subcarriers > MAX_SUBCARRIERS {
return Err(ParseError::InvalidSubcarrierCount {
count: subcarrier_count,
count: n_subcarriers,
max: MAX_SUBCARRIERS,
});
}
if iq_length % 2 != 0 {
return Err(ParseError::IqLengthMismatch {
expected: subcarrier_count * 2,
got: iq_length,
});
}
// Frequency MHz (offset 8, 4 bytes LE)
let channel_freq_mhz = cursor.read_u32::<LittleEndian>().map_err(|_| ParseError::ByteError {
offset: 8,
message: "Failed to read frequency".into(),
})?;
// Sequence number (offset 12, 4 bytes LE)
let sequence = cursor.read_u32::<LittleEndian>().map_err(|_| ParseError::ByteError {
offset: 12,
message: "Failed to read sequence number".into(),
})?;
// RSSI (offset 16, 1 byte signed)
let rssi_dbm = cursor.read_i8().map_err(|_| ParseError::ByteError {
offset: 16,
message: "Failed to read RSSI".into(),
})?;
// Noise floor (offset 17, 1 byte signed)
let noise_floor_dbm = cursor.read_i8().map_err(|_| ParseError::ByteError {
offset: 17,
message: "Failed to read noise floor".into(),
})?;
// Reserved (offset 18, 2 bytes) — skip
let _reserved = cursor.read_u16::<LittleEndian>().map_err(|_| ParseError::ByteError {
offset: 18,
message: "Failed to read reserved bytes".into(),
})?;
// I/Q data: n_antennas * n_subcarriers * 2 bytes
let iq_pair_count = n_antennas as usize * n_subcarriers;
let iq_byte_count = iq_pair_count * 2;
let total_frame_size = HEADER_SIZE + iq_byte_count;
// Check we have enough bytes for the I/Q data
let total_frame_size = 20 + iq_length;
if data.len() < total_frame_size {
return Err(ParseError::InsufficientData {
needed: total_frame_size,
@@ -150,33 +148,34 @@ impl Esp32CsiParser {
});
}
// Parse I/Q pairs
let iq_start = 20;
let mut subcarriers = Vec::with_capacity(subcarrier_count);
// Parse I/Q pairs — stored as [ant0_sc0_I, ant0_sc0_Q, ant0_sc1_I, ant0_sc1_Q, ..., ant1_sc0_I, ...]
let iq_start = HEADER_SIZE;
let mut subcarriers = Vec::with_capacity(iq_pair_count);
// Subcarrier index mapping for 20 MHz: -28 to +28 (skipping 0)
let half = subcarrier_count as i16 / 2;
let half = n_subcarriers as i16 / 2;
for sc_idx in 0..subcarrier_count {
let byte_offset = iq_start + sc_idx * 2;
let i_val = data[byte_offset] as i8 as i16;
let q_val = data[byte_offset + 1] as i8 as i16;
for ant in 0..n_antennas as usize {
for sc_idx in 0..n_subcarriers {
let byte_offset = iq_start + (ant * n_subcarriers + sc_idx) * 2;
let i_val = data[byte_offset] as i8 as i16;
let q_val = data[byte_offset + 1] as i8 as i16;
let index = if (sc_idx as i16) < half {
-(half - sc_idx as i16)
} else {
sc_idx as i16 - half + 1
};
let index = if (sc_idx as i16) < half {
-(half - sc_idx as i16)
} else {
sc_idx as i16 - half + 1
};
subcarriers.push(SubcarrierData {
i: i_val,
q: q_val,
index,
});
subcarriers.push(SubcarrierData {
i: i_val,
q: q_val,
index,
});
}
}
// Determine bandwidth from subcarrier count
let bandwidth = match subcarrier_count {
let bandwidth = match n_subcarriers {
0..=56 => Bandwidth::Bw20,
57..=114 => Bandwidth::Bw40,
115..=242 => Bandwidth::Bw80,
@@ -186,16 +185,17 @@ impl Esp32CsiParser {
let frame = CsiFrame {
metadata: CsiMetadata {
timestamp: Utc::now(),
rssi,
noise_floor,
channel,
secondary_channel,
node_id,
n_antennas,
n_subcarriers: n_subcarriers as u16,
channel_freq_mhz,
rssi_dbm,
noise_floor_dbm,
bandwidth,
antenna_config: AntennaConfig {
tx_antennas: 1,
rx_antennas: 1,
rx_antennas: n_antennas,
},
source_mac: Some(mac),
sequence,
},
subcarriers,
@@ -204,7 +204,7 @@ impl Esp32CsiParser {
Ok((frame, total_frame_size))
}
/// Parse multiple frames from a byte buffer (e.g., from a serial read).
/// Parse multiple frames from a byte buffer (e.g., from a UDP read).
///
/// Returns all successfully parsed frames and the total bytes consumed.
pub fn parse_stream(data: &[u8]) -> (Vec<CsiFrame>, usize) {
@@ -244,28 +244,35 @@ impl Esp32CsiParser {
mod tests {
use super::*;
/// Build a valid ESP32 CSI frame with known I/Q values.
fn build_test_frame(subcarrier_pairs: &[(i8, i8)]) -> Vec<u8> {
/// Build a valid ADR-018 ESP32 CSI frame with known parameters.
fn build_test_frame(node_id: u8, n_antennas: u8, subcarrier_pairs: &[(i8, i8)]) -> Vec<u8> {
let n_subcarriers = if n_antennas == 0 {
subcarrier_pairs.len()
} else {
subcarrier_pairs.len() / n_antennas as usize
};
let mut buf = Vec::new();
// Magic
// Magic (offset 0)
buf.extend_from_slice(&ESP32_CSI_MAGIC.to_le_bytes());
// Sequence
// Node ID (offset 4)
buf.push(node_id);
// Number of antennas (offset 5)
buf.push(n_antennas);
// Number of subcarriers (offset 6, LE u16)
buf.extend_from_slice(&(n_subcarriers as u16).to_le_bytes());
// Frequency MHz (offset 8, LE u32)
buf.extend_from_slice(&2437u32.to_le_bytes());
// Sequence number (offset 12, LE u32)
buf.extend_from_slice(&1u32.to_le_bytes());
// Channel
buf.push(6);
// Secondary channel
buf.push(0);
// RSSI
// RSSI (offset 16, i8)
buf.push((-50i8) as u8);
// Noise floor
// Noise floor (offset 17, i8)
buf.push((-95i8) as u8);
// I/Q length
let iq_len = (subcarrier_pairs.len() * 2) as u16;
buf.extend_from_slice(&iq_len.to_le_bytes());
// MAC
buf.extend_from_slice(&[0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF]);
// I/Q data
// Reserved (offset 18, 2 bytes)
buf.extend_from_slice(&[0u8; 2]);
// I/Q data (offset 20)
for (i, q) in subcarrier_pairs {
buf.push(*i as u8);
buf.push(*q as u8);
@@ -276,15 +283,19 @@ mod tests {
#[test]
fn test_parse_valid_frame() {
// 1 antenna, 56 subcarriers
let pairs: Vec<(i8, i8)> = (0..56).map(|i| (i as i8, (i * 2 % 127) as i8)).collect();
let data = build_test_frame(&pairs);
let data = build_test_frame(1, 1, &pairs);
let (frame, consumed) = Esp32CsiParser::parse_frame(&data).unwrap();
assert_eq!(consumed, 20 + 112);
assert_eq!(consumed, HEADER_SIZE + 56 * 2);
assert_eq!(frame.subcarrier_count(), 56);
assert_eq!(frame.metadata.rssi, -50);
assert_eq!(frame.metadata.channel, 6);
assert_eq!(frame.metadata.node_id, 1);
assert_eq!(frame.metadata.n_antennas, 1);
assert_eq!(frame.metadata.n_subcarriers, 56);
assert_eq!(frame.metadata.rssi_dbm, -50);
assert_eq!(frame.metadata.channel_freq_mhz, 2437);
assert_eq!(frame.metadata.bandwidth, Bandwidth::Bw20);
assert!(frame.is_valid());
}
@@ -298,7 +309,7 @@ mod tests {
#[test]
fn test_parse_invalid_magic() {
let mut data = build_test_frame(&[(10, 20)]);
let mut data = build_test_frame(1, 1, &[(10, 20)]);
// Corrupt magic
data[0] = 0xFF;
let result = Esp32CsiParser::parse_frame(&data);
@@ -308,10 +319,10 @@ mod tests {
#[test]
fn test_amplitude_phase_from_known_iq() {
let pairs = vec![(100i8, 0i8), (0, 50), (30, 40)];
let data = build_test_frame(&pairs);
let data = build_test_frame(1, 1, &pairs);
let (frame, _) = Esp32CsiParser::parse_frame(&data).unwrap();
let (amps, phases) = frame.to_amplitude_phase();
let (amps, _phases) = frame.to_amplitude_phase();
assert_eq!(amps.len(), 3);
// I=100, Q=0 -> amplitude=100
@@ -325,8 +336,8 @@ mod tests {
#[test]
fn test_parse_stream_with_multiple_frames() {
let pairs: Vec<(i8, i8)> = (0..4).map(|i| (10 + i, 20 + i)).collect();
let frame1 = build_test_frame(&pairs);
let frame2 = build_test_frame(&pairs);
let frame1 = build_test_frame(1, 1, &pairs);
let frame2 = build_test_frame(2, 1, &pairs);
let mut combined = Vec::new();
combined.extend_from_slice(&frame1);
@@ -334,12 +345,14 @@ mod tests {
let (frames, _consumed) = Esp32CsiParser::parse_stream(&combined);
assert_eq!(frames.len(), 2);
assert_eq!(frames[0].metadata.node_id, 1);
assert_eq!(frames[1].metadata.node_id, 2);
}
#[test]
fn test_parse_stream_with_garbage() {
let pairs: Vec<(i8, i8)> = (0..4).map(|i| (10 + i, 20 + i)).collect();
let frame = build_test_frame(&pairs);
let frame = build_test_frame(1, 1, &pairs);
let mut data = Vec::new();
data.extend_from_slice(&[0xFF, 0xFF, 0xFF]); // garbage
@@ -350,14 +363,23 @@ mod tests {
}
#[test]
fn test_mac_address_parsed() {
let pairs = vec![(10i8, 20i8)];
let data = build_test_frame(&pairs);
let (frame, _) = Esp32CsiParser::parse_frame(&data).unwrap();
fn test_multi_antenna_frame() {
// 3 antennas, 4 subcarriers each = 12 I/Q pairs total
let mut pairs = Vec::new();
for ant in 0..3u8 {
for sc in 0..4u8 {
pairs.push(((ant * 10 + sc) as i8, ((ant * 10 + sc) * 2) as i8));
}
}
assert_eq!(
frame.metadata.source_mac,
Some([0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF])
);
let data = build_test_frame(5, 3, &pairs);
let (frame, consumed) = Esp32CsiParser::parse_frame(&data).unwrap();
assert_eq!(consumed, HEADER_SIZE + 12 * 2);
assert_eq!(frame.metadata.node_id, 5);
assert_eq!(frame.metadata.n_antennas, 3);
assert_eq!(frame.metadata.n_subcarriers, 4);
assert_eq!(frame.subcarrier_count(), 12); // 3 antennas * 4 subcarriers
assert_eq!(frame.metadata.antenna_config.rx_antennas, 3);
}
}

15
rust-port/wifi-densepose-rs/crates/wifi-densepose-hardware/src/lib.rs
View File

@@ -3,11 +3,9 @@
//! This crate provides platform-agnostic types and parsers for WiFi CSI data
//! from various hardware sources:
//!
//! - **ESP32/ESP32-S3**: Parses binary CSI frames from ESP-IDF `wifi_csi_info_t`
//! streamed over serial (UART) or UDP
//! - **Intel 5300**: Parses CSI log files from the Linux CSI Tool
//! - **Linux WiFi**: Reads RSSI/signal info from standard Linux wireless interfaces
//! for commodity sensing (ADR-013)
//! - **ESP32/ESP32-S3**: Parses ADR-018 binary CSI frames streamed over UDP
//! - **UDP Aggregator**: Receives frames from multiple ESP32 nodes (ADR-018 Layer 2)
//! - **Bridge**: Converts CsiFrame → CsiData for the detection pipeline (ADR-018 Layer 3)
//!
//! # Design Principles
//!
@@ -21,8 +19,8 @@
//! ```rust
//! use wifi_densepose_hardware::{CsiFrame, Esp32CsiParser, ParseError};
//!
//! // Parse ESP32 CSI data from serial bytes
//! let raw_bytes: &[u8] = &[/* ESP32 CSI binary frame */];
//! // Parse ESP32 CSI data from UDP bytes
//! let raw_bytes: &[u8] = &[/* ADR-018 binary frame */];
//! match Esp32CsiParser::parse_frame(raw_bytes) {
//! Ok((frame, consumed)) => {
//! println!("Parsed {} subcarriers ({} bytes)", frame.subcarrier_count(), consumed);
@@ -39,7 +37,10 @@
mod csi_frame;
mod error;
mod esp32_parser;
pub mod aggregator;
mod bridge;
pub use csi_frame::{CsiFrame, CsiMetadata, SubcarrierData, Bandwidth, AntennaConfig};
pub use error::ParseError;
pub use esp32_parser::Esp32CsiParser;
pub use bridge::CsiData;

167
v1/src/hardware/csi_extractor.py
View File

@@ -1,6 +1,7 @@
"""CSI data extraction from WiFi hardware using Test-Driven Development approach."""
import asyncio
import struct
import numpy as np
from datetime import datetime, timezone
from typing import Dict, Any, Optional, Callable, Protocol
@@ -129,6 +130,106 @@ class ESP32CSIParser:
raise CSIParseError(f"Failed to parse ESP32 data: {e}")
class ESP32BinaryParser:
"""Parser for ADR-018 binary CSI frames from ESP32 nodes.
Binary frame format:
Offset Size Field
0 4 Magic: 0xC5110001 (LE)
4 1 Node ID
5 1 Number of antennas
6 2 Number of subcarriers (LE u16)
8 4 Frequency MHz (LE u32)
12 4 Sequence number (LE u32)
16 1 RSSI (i8)
17 1 Noise floor (i8)
18 2 Reserved
20 N*2 I/Q pairs (n_antennas * n_subcarriers * 2 bytes, signed i8)
"""
MAGIC = 0xC5110001
HEADER_SIZE = 20
HEADER_FMT = '<IBBHIIBB2x' # magic, node_id, n_ant, n_sc, freq, seq, rssi, noise
def parse(self, raw_data: bytes) -> CSIData:
"""Parse an ADR-018 binary frame into CSIData.
Args:
raw_data: Raw binary frame bytes.
Returns:
Parsed CSI data with amplitude/phase arrays shaped (n_antennas, n_subcarriers).
Raises:
CSIParseError: If frame is too short, has invalid magic, or malformed I/Q data.
"""
if len(raw_data) < self.HEADER_SIZE:
raise CSIParseError(
f"Frame too short: need {self.HEADER_SIZE} bytes, got {len(raw_data)}"
)
magic, node_id, n_antennas, n_subcarriers, freq_mhz, sequence, rssi_u8, noise_u8 = \
struct.unpack_from(self.HEADER_FMT, raw_data, 0)
if magic != self.MAGIC:
raise CSIParseError(
f"Invalid magic: expected 0x{self.MAGIC:08X}, got 0x{magic:08X}"
)
# Convert unsigned bytes to signed i8
rssi = rssi_u8 if rssi_u8 < 128 else rssi_u8 - 256
noise_floor = noise_u8 if noise_u8 < 128 else noise_u8 - 256
iq_count = n_antennas * n_subcarriers
iq_bytes = iq_count * 2
expected_len = self.HEADER_SIZE + iq_bytes
if len(raw_data) < expected_len:
raise CSIParseError(
f"Frame too short for I/Q data: need {expected_len} bytes, got {len(raw_data)}"
)
# Parse I/Q pairs as signed bytes
iq_raw = struct.unpack_from(f'<{iq_count * 2}b', raw_data, self.HEADER_SIZE)
i_vals = np.array(iq_raw[0::2], dtype=np.float64).reshape(n_antennas, n_subcarriers)
q_vals = np.array(iq_raw[1::2], dtype=np.float64).reshape(n_antennas, n_subcarriers)
amplitude = np.sqrt(i_vals ** 2 + q_vals ** 2)
phase = np.arctan2(q_vals, i_vals)
snr = float(rssi - noise_floor)
frequency = float(freq_mhz) * 1e6
bandwidth = 20e6 # default; could infer from n_subcarriers
if n_subcarriers <= 56:
bandwidth = 20e6
elif n_subcarriers <= 114:
bandwidth = 40e6
elif n_subcarriers <= 242:
bandwidth = 80e6
else:
bandwidth = 160e6
return CSIData(
timestamp=datetime.now(tz=timezone.utc),
amplitude=amplitude,
phase=phase,
frequency=frequency,
bandwidth=bandwidth,
num_subcarriers=n_subcarriers,
num_antennas=n_antennas,
snr=snr,
metadata={
'source': 'esp32_binary',
'node_id': node_id,
'sequence': sequence,
'rssi_dbm': rssi,
'noise_floor_dbm': noise_floor,
'channel_freq_mhz': freq_mhz,
}
)
class RouterCSIParser:
"""Parser for router CSI data format."""
@@ -203,7 +304,10 @@ class CSIExtractor:
# Create appropriate parser
if self.hardware_type == 'esp32':
self.parser = ESP32CSIParser()
if config.get('parser_format') == 'binary':
self.parser = ESP32BinaryParser()
else:
self.parser = ESP32CSIParser()
elif self.hardware_type == 'router':
self.parser = RouterCSIParser()
else:
@@ -352,6 +456,61 @@ class CSIExtractor:
pass
async def _read_raw_data(self) -> bytes:
"""Read raw data from hardware (to be implemented by subclasses)."""
# Placeholder implementation for testing
return b"CSI_DATA:1234567890,3,56,2400,20,15.5,[1.0,2.0,3.0],[0.5,1.5,2.5]"
"""Read raw data from hardware.
When parser_format='binary', reads from the configured UDP socket.
Otherwise returns placeholder text data for legacy compatibility.
Raises:
CSIExtractionError: If UDP read times out or fails.
"""
if self.config.get('parser_format') == 'binary':
return await self._read_udp_data()
# Placeholder implementation for legacy text-mode testing
return b"CSI_DATA:1234567890,3,56,2400,20,15.5,[1.0,2.0,3.0],[0.5,1.5,2.5]"
async def _read_udp_data(self) -> bytes:
"""Read a single UDP packet from the aggregator.
Raises:
CSIExtractionError: If read times out or connection fails.
"""
host = self.config.get('aggregator_host', '0.0.0.0')
port = self.config.get('aggregator_port', 5005)
loop = asyncio.get_event_loop()
# Create UDP endpoint if not already cached
if not hasattr(self, '_udp_transport'):
self._udp_future: asyncio.Future = loop.create_future()
class _UdpProtocol(asyncio.DatagramProtocol):
def __init__(self, future):
self._future = future
def datagram_received(self, data, addr):
if not self._future.done():
self._future.set_result(data)
def error_received(self, exc):
if not self._future.done():
self._future.set_exception(exc)
transport, protocol = await loop.create_datagram_endpoint(
lambda: _UdpProtocol(self._udp_future),
local_addr=(host, port),
)
self._udp_transport = transport
self._udp_protocol = protocol
try:
data = await asyncio.wait_for(self._udp_future, timeout=self.timeout)
# Reset future for next read
self._udp_future = loop.create_future()
self._udp_protocol._future = self._udp_future
return data
except asyncio.TimeoutError:
raise CSIExtractionError(
f"UDP read timed out after {self.timeout}s. "
f"Ensure the aggregator is running and sending to {host}:{port}."
)

206
v1/tests/unit/test_esp32_binary_parser.py Normal file
View File

@@ -0,0 +1,206 @@
"""Tests for ESP32BinaryParser (ADR-018 binary frame format)."""
import asyncio
import math
import socket
import struct
import threading
import time
import numpy as np
import pytest
import sys
import os
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'src'))
from hardware.csi_extractor import (
ESP32BinaryParser,
CSIExtractor,
CSIParseError,
CSIExtractionError,
)
# ADR-018 constants
MAGIC = 0xC5110001
HEADER_FMT = '<IBBHIIBB2x'
HEADER_SIZE = 20
def build_binary_frame(
node_id: int = 1,
n_antennas: int = 1,
n_subcarriers: int = 4,
freq_mhz: int = 2437,
sequence: int = 0,
rssi: int = -50,
noise_floor: int = -90,
iq_pairs: list = None,
) -> bytes:
"""Build an ADR-018 binary frame for testing."""
if iq_pairs is None:
iq_pairs = [(i % 50, (i * 2) % 50) for i in range(n_antennas * n_subcarriers)]
rssi_u8 = rssi & 0xFF
noise_u8 = noise_floor & 0xFF
header = struct.pack(
HEADER_FMT,
MAGIC,
node_id,
n_antennas,
n_subcarriers,
freq_mhz,
sequence,
rssi_u8,
noise_u8,
)
iq_data = b''
for i_val, q_val in iq_pairs:
iq_data += struct.pack('<bb', i_val, q_val)
return header + iq_data
class TestESP32BinaryParser:
"""Tests for ESP32BinaryParser."""
def setup_method(self):
self.parser = ESP32BinaryParser()
def test_parse_valid_binary_frame(self):
"""Parse a well-formed ADR-018 binary frame."""
iq = [(3, 4), (0, 10), (5, 12), (7, 0)]
frame_bytes = build_binary_frame(
node_id=1, n_antennas=1, n_subcarriers=4,
freq_mhz=2437, sequence=42, rssi=-50, noise_floor=-90,
iq_pairs=iq,
)
result = self.parser.parse(frame_bytes)
assert result.num_antennas == 1
assert result.num_subcarriers == 4
assert result.amplitude.shape == (1, 4)
assert result.phase.shape == (1, 4)
assert result.metadata['node_id'] == 1
assert result.metadata['sequence'] == 42
assert result.metadata['rssi_dbm'] == -50
assert result.metadata['noise_floor_dbm'] == -90
assert result.metadata['channel_freq_mhz'] == 2437
# Check amplitude for I=3, Q=4 -> sqrt(9+16) = 5.0
assert abs(result.amplitude[0, 0] - 5.0) < 0.001
# I=0, Q=10 -> 10.0
assert abs(result.amplitude[0, 1] - 10.0) < 0.001
def test_parse_frame_too_short(self):
"""Reject frames shorter than the 20-byte header."""
with pytest.raises(CSIParseError, match="too short"):
self.parser.parse(b'\x00' * 10)
def test_parse_invalid_magic(self):
"""Reject frames with wrong magic number."""
bad_frame = build_binary_frame()
# Corrupt magic
bad_frame = b'\xFF\xFF\xFF\xFF' + bad_frame[4:]
with pytest.raises(CSIParseError, match="Invalid magic"):
self.parser.parse(bad_frame)
def test_parse_multi_antenna_frame(self):
"""Parse a frame with 3 antennas and 4 subcarriers."""
n_ant = 3
n_sc = 4
iq = [(i + 1, i + 2) for i in range(n_ant * n_sc)]
frame_bytes = build_binary_frame(
node_id=5, n_antennas=n_ant, n_subcarriers=n_sc,
iq_pairs=iq,
)
result = self.parser.parse(frame_bytes)
assert result.num_antennas == 3
assert result.num_subcarriers == 4
assert result.amplitude.shape == (3, 4)
assert result.phase.shape == (3, 4)
def test_udp_read_with_mock_server(self):
"""Send a frame via UDP and verify CSIExtractor receives it."""
# Find a free port
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('127.0.0.1', 0))
port = sock.getsockname()[1]
sock.close()
frame_bytes = build_binary_frame(
node_id=3, n_antennas=1, n_subcarriers=4,
freq_mhz=2412, sequence=99,
)
config = {
'hardware_type': 'esp32',
'parser_format': 'binary',
'sampling_rate': 100,
'buffer_size': 2048,
'timeout': 2,
'aggregator_host': '127.0.0.1',
'aggregator_port': port,
}
extractor = CSIExtractor(config)
async def run_test():
# Connect
await extractor.connect()
# Send frame after a short delay from a background thread
def send():
time.sleep(0.2)
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.sendto(frame_bytes, ('127.0.0.1', port))
s.close()
sender = threading.Thread(target=send, daemon=True)
sender.start()
result = await extractor.extract_csi()
sender.join(timeout=2)
assert result.metadata['node_id'] == 3
assert result.metadata['sequence'] == 99
assert result.num_subcarriers == 4
await extractor.disconnect()
asyncio.run(run_test())
def test_udp_timeout(self):
"""Verify timeout when no UDP server is sending data."""
# Find a free port (nothing will send to it)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('127.0.0.1', 0))
port = sock.getsockname()[1]
sock.close()
config = {
'hardware_type': 'esp32',
'parser_format': 'binary',
'sampling_rate': 100,
'buffer_size': 2048,
'timeout': 0.5,
'retry_attempts': 1,
'aggregator_host': '127.0.0.1',
'aggregator_port': port,
}
extractor = CSIExtractor(config)
async def run_test():
await extractor.connect()
with pytest.raises(CSIExtractionError, match="timed out"):
await extractor.extract_csi()
await extractor.disconnect()
asyncio.run(run_test())