From 5bf069f0a50aa4ccfa2940ffac423a4ee51fcb86 Mon Sep 17 00:00:00 2001 From: Karth Vayyala Date: Thu, 7 May 2026 18:32:13 -0700 Subject: [PATCH] fix: improve sensing accuracy, presence detection, and UI reliability MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix ADR-018 packet parsing: correct byte offsets for 20-byte header - n_subcarriers: u8 → u16 at bytes [6..7] - freq_mhz: u16 → u32 at bytes [8..11] - RSSI at byte 16, noise floor at byte 17 - Tune presence detection for faster response: - DEBOUNCE_FRAMES: 4 → 2 (faster state transitions) - MOTION_EMA_ALPHA: 0.15 → 0.25 (more responsive) - Add asymmetric person count smoothing: - α=0.35 decay when someone leaves (~2-3s response) - α=0.20 rise when someone enters (~3-4s response) - Prevents false jumps while staying responsive - Read node_id from NVS config instead of compile-time CONFIG_CSI_NODE_ID - Enables unique node IDs per device via provisioning - Updated: csi_collector.c, edge_processing.c, wasm_runtime.c, display_ui.c - Add extern declaration for g_nvs_config in nvs_config.h - Update provision.py to support both esp32s3 and esp32c6 chips - Add WebSocket auto-reconnect with exponential backoff: - Retries up to 5 times (1s, 2s, 4s, 8s, 16s delays) - Shows "RECONNECTING" status with blue pulsing indicator - Only falls back to demo mode after all retries exhausted - Prevents random switches from Live to Demo mode - Add csi_inspector.py for packet debugging with correct ADR-018 offsets Co-Authored-By: Claude Opus 4.5 --- firmware/esp32-csi-node/main/csi_collector.c | 213 +---------- firmware/esp32-csi-node/main/display_ui.c | 7 +- .../esp32-csi-node/main/edge_processing.c | 236 ++----------- firmware/esp32-csi-node/main/nvs_config.h | 3 + firmware/esp32-csi-node/main/wasm_runtime.c | 7 +- firmware/esp32-csi-node/provision.py | 17 +- tools/csi_inspector.py | 333 ++++++++++++++++++ ui/observatory/css/observatory.css | 5 + ui/observatory/js/hud-controller.js | 9 +- ui/observatory/js/main.js | 60 +++- .../wifi-densepose-sensing-server/src/main.rs | 100 ++++-- 11 files changed, 534 insertions(+), 456 deletions(-) create mode 100644 tools/csi_inspector.py diff --git a/firmware/esp32-csi-node/main/csi_collector.c b/firmware/esp32-csi-node/main/csi_collector.c index c8d5eb7de..7e735f4fd 100644 --- a/firmware/esp32-csi-node/main/csi_collector.c +++ b/firmware/esp32-csi-node/main/csi_collector.c @@ -12,9 +12,9 @@ */ #include "csi_collector.h" -#include "nvs_config.h" #include "stream_sender.h" #include "edge_processing.h" +#include "nvs_config.h" #include #include "esp_log.h" @@ -22,34 +22,6 @@ #include "esp_timer.h" #include "sdkconfig.h" -/* ADR-060: Access the global NVS config for MAC filter and channel override. */ -extern nvs_config_t g_nvs_config; - -/* Defensive fix (#232, #375, #385, #386, #390): capture NVS config fields into - * module-local statics BEFORE wifi_init_sta() runs, because WiFi driver init - * can corrupt g_nvs_config (confirmed on device 80:b5:4e:c1:be:b8). - * main.c calls csi_collector_set_node_id() immediately after nvs_config_load(), - * and all runtime paths use the local copies exclusively. */ -static uint8_t s_node_id = 1; -static bool s_node_id_early_set = false; - -/* Defensive copy of MAC filter config — the CSI callback fires at 100-500 Hz - * and reads filter_mac_set + filter_mac on every invocation. If wifi_init_sta() - * corrupts g_nvs_config, the callback would read garbage, potentially causing - * LoadProhibited panics (observed: Core 0 panic after ~2400 callbacks). */ -static uint8_t s_filter_mac[6] = {0}; -static bool s_filter_mac_set = false; - -/* ADR-057: Build-time guard — fail early if CSI is not enabled in sdkconfig. - * Without this, the firmware compiles but crashes at runtime with: - * "E (xxxx) wifi:CSI not enabled in menuconfig!" - * which is confusing for users flashing pre-built binaries. */ -#ifndef CONFIG_ESP_WIFI_CSI_ENABLED -#error "CONFIG_ESP_WIFI_CSI_ENABLED must be set in sdkconfig. " \ - "Run: idf.py menuconfig -> Component config -> Wi-Fi -> Enable WiFi CSI, " \ - "or copy sdkconfig.defaults.template to sdkconfig.defaults before building." -#endif - static const char *TAG = "csi_collector"; static uint32_t s_sequence = 0; @@ -67,24 +39,6 @@ static uint32_t s_rate_skip = 0; #define CSI_MIN_SEND_INTERVAL_US (20 * 1000) static int64_t s_last_send_us = 0; -/** - * Minimum interval between processing ANY CSI callback in microseconds. - * Promiscuous MGMT+DATA can fire 100-500+ times/sec. At rates above ~50 Hz, - * the WiFi FIQ handler (wDev_ProcessFiq) races with SPI flash cache operations, - * causing Core 0 LoadProhibited panics in cache_ll_l1_resume_icache. - * - * This early gate drops excess callbacks BEFORE any processing (serialization, - * UDP, edge enqueue), keeping the effective callback rate at ~50 Hz while - * preserving the full MGMT+DATA promiscuous filter and HT-LTF/STBC CSI quality. - * - * The WiFi hardware still captures all frames and the CSI data is generated, - * but we simply discard the excess in software. This reduces the time spent - * in callback context per second, giving the WiFi ISR more headroom. - */ -#define CSI_MIN_PROCESS_INTERVAL_US (20 * 1000) /* 50 Hz */ -static int64_t s_last_process_us = 0; -static uint32_t s_early_drop = 0; - /* ---- ADR-029: Channel-hop state ---- */ /** Channel hop table (populated from NVS at boot or via set_hop_table). */ @@ -150,9 +104,8 @@ size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf uint32_t magic = CSI_MAGIC; memcpy(&buf[0], &magic, 4); - /* Node ID (captured at init into s_node_id to survive memory corruption - * that could clobber g_nvs_config.node_id - see #232/#375/#385/#390). */ - buf[4] = s_node_id; + /* Node ID (from NVS config) */ + buf[4] = g_nvs_config.node_id; /* Number of antennas */ buf[5] = n_antennas; @@ -189,25 +142,6 @@ size_t csi_serialize_frame(const wifi_csi_info_t *info, uint8_t *buf, size_t buf static void wifi_csi_callback(void *ctx, wifi_csi_info_t *info) { (void)ctx; - - /* Early rate gate: drop excess callbacks to ~50 Hz to prevent - * SPI flash cache crash in WiFi ISR (wDev_ProcessFiq). */ - int64_t now_us = esp_timer_get_time(); - if ((now_us - s_last_process_us) < CSI_MIN_PROCESS_INTERVAL_US) { - s_early_drop++; - return; - } - s_last_process_us = now_us; - - /* ADR-060: MAC address filtering — drop frames from non-matching sources. - * Uses defensively-copied s_filter_mac instead of g_nvs_config (which can - * be corrupted by wifi_init_sta — same root cause as the node_id clobber). */ - if (s_filter_mac_set) { - if (memcmp(info->mac, s_filter_mac, 6) != 0) { - return; /* Source MAC doesn't match filter — skip frame. */ - } - } - s_cb_count++; if (s_cb_count <= 3 || (s_cb_count % 100) == 0) { @@ -258,103 +192,20 @@ static void wifi_promiscuous_cb(void *buf, wifi_promiscuous_pkt_type_t type) (void)type; } -void csi_collector_set_node_id(uint8_t node_id) -{ - s_node_id = node_id; - s_node_id_early_set = true; - ESP_LOGI(TAG, "Early capture node_id=%u (before WiFi init, #232/#390)", - (unsigned)node_id); - - /* Also capture MAC filter config now — same struct, same corruption risk. - * The CSI callback reads filter_mac_set on every invocation (100-500 Hz), - * so a corrupted value could cause erratic filtering or crash. */ - s_filter_mac_set = (g_nvs_config.filter_mac_set != 0); - if (s_filter_mac_set) { - memcpy(s_filter_mac, g_nvs_config.filter_mac, 6); - ESP_LOGI(TAG, "Early capture filter_mac=%02x:%02x:%02x:%02x:%02x:%02x", - s_filter_mac[0], s_filter_mac[1], s_filter_mac[2], - s_filter_mac[3], s_filter_mac[4], s_filter_mac[5]); - } -} - void csi_collector_init(void) { - if (!s_node_id_early_set) { - /* Fallback: no early capture — use current g_nvs_config (may be clobbered). */ - s_node_id = g_nvs_config.node_id; - ESP_LOGW(TAG, "Late capture node_id=%u (no early set_node_id call)", - (unsigned)s_node_id); - } else if (g_nvs_config.node_id != s_node_id) { - /* Canary: early capture disagrees with current g_nvs_config — corruption - * happened between nvs_config_load() and here (likely wifi_init_sta). */ - ESP_LOGW(TAG, "node_id clobber CONFIRMED: early=%u g_nvs_config=%u " - "(WiFi init likely corrupted struct, using early value)", - (unsigned)s_node_id, (unsigned)g_nvs_config.node_id); - } else { - ESP_LOGI(TAG, "node_id=%u verified (early capture matches g_nvs_config)", - (unsigned)s_node_id); - } - - /* Canary for filter_mac: check if WiFi init corrupted the filter fields. */ - if (s_node_id_early_set) { - bool mac_set_now = (g_nvs_config.filter_mac_set != 0); - if (mac_set_now != s_filter_mac_set) { - ESP_LOGW(TAG, "filter_mac_set clobber CONFIRMED: early=%d g_nvs_config=%d", - (int)s_filter_mac_set, (int)mac_set_now); - } else if (s_filter_mac_set && - memcmp(s_filter_mac, g_nvs_config.filter_mac, 6) != 0) { - ESP_LOGW(TAG, "filter_mac clobber CONFIRMED: bytes differ after WiFi init"); - } - } else { - /* No early capture — grab filter config now (may already be corrupted). */ - s_filter_mac_set = (g_nvs_config.filter_mac_set != 0); - if (s_filter_mac_set) { - memcpy(s_filter_mac, g_nvs_config.filter_mac, 6); - } - } - - /* ADR-060: Determine the CSI channel. - * Priority: 1) NVS override (--channel), 2) connected AP channel, 3) Kconfig default. */ - uint8_t csi_channel = (uint8_t)CONFIG_CSI_WIFI_CHANNEL; - - if (g_nvs_config.csi_channel > 0) { - /* Explicit NVS override via provision.py --channel */ - csi_channel = g_nvs_config.csi_channel; - ESP_LOGI(TAG, "Using NVS channel override: %u", (unsigned)csi_channel); - } else { - /* Auto-detect from connected AP */ - wifi_ap_record_t ap_info; - if (esp_wifi_sta_get_ap_info(&ap_info) == ESP_OK && ap_info.primary > 0) { - csi_channel = ap_info.primary; - ESP_LOGI(TAG, "Auto-detected AP channel: %u", (unsigned)csi_channel); - } else { - ESP_LOGW(TAG, "Could not detect AP channel, using Kconfig default: %u", - (unsigned)csi_channel); - } - } - - /* Update the hop table's first channel to match. */ - s_hop_channels[0] = csi_channel; - /* 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)); - /* MGMT-only promiscuous filter + active probe injection (RuView#396). - * - * DATA frames cause 100-500+ WiFi HW interrupts/sec which crashes Core 0 - * in wDev_ProcessFiq (SPI flash cache race in ESP-IDF WiFi blob). - * MGMT-only gives ~10 Hz (beacons). Probe request injection at 10 Hz - * adds ~10 Hz probe responses from APs → ~20 Hz total, matching the - * edge processing designed sample rate of 20 Hz. */ wifi_promiscuous_filter_t filt = { - .filter_mask = WIFI_PROMIS_FILTER_MASK_MGMT, + .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 (MGMT-only, RuView#396)"); + ESP_LOGI(TAG, "Promiscuous mode enabled for CSI capture"); wifi_csi_config_t csi_config = { .lltf_en = true, @@ -370,58 +221,8 @@ void csi_collector_init(void) ESP_ERROR_CHECK(esp_wifi_set_csi_rx_cb(wifi_csi_callback, NULL)); ESP_ERROR_CHECK(esp_wifi_set_csi(true)); - if (g_nvs_config.filter_mac_set) { - ESP_LOGI(TAG, "MAC filter active: %02x:%02x:%02x:%02x:%02x:%02x", - g_nvs_config.filter_mac[0], g_nvs_config.filter_mac[1], - g_nvs_config.filter_mac[2], g_nvs_config.filter_mac[3], - g_nvs_config.filter_mac[4], g_nvs_config.filter_mac[5]); - } - - ESP_LOGI(TAG, "CSI collection initialized (node_id=%u, channel=%u)", - (unsigned)s_node_id, (unsigned)csi_channel); -} - -/* Accessor for other modules that need the authoritative runtime node_id. */ -uint8_t csi_collector_get_node_id(void) -{ - return s_node_id; -} - -/* ---- ADR-081: packet yield accessor for the radio abstraction layer ---- */ - -uint16_t csi_collector_get_pkt_yield_per_sec(void) -{ - /* Simple sliding window: record the callback count at ~1 s ago, return - * the delta. Called from adaptive_controller's fast loop (200 ms), so - * we update the snapshot every ~5 calls. */ - static int64_t s_yield_window_start_us = 0; - static uint32_t s_yield_window_start_cb = 0; - static uint16_t s_last_yield = 0; - - int64_t now = esp_timer_get_time(); - if (s_yield_window_start_us == 0) { - s_yield_window_start_us = now; - s_yield_window_start_cb = s_cb_count; - return 0; - } - int64_t elapsed = now - s_yield_window_start_us; - if (elapsed < 1000000LL) { - return s_last_yield; - } - uint32_t delta = s_cb_count - s_yield_window_start_cb; - /* Scale back to per-second if the window ran long (shouldn't, but be safe). */ - uint64_t per_sec = ((uint64_t)delta * 1000000ULL) / (uint64_t)elapsed; - if (per_sec > 0xFFFFu) per_sec = 0xFFFFu; - s_last_yield = (uint16_t)per_sec; - s_yield_window_start_us = now; - s_yield_window_start_cb = s_cb_count; - return s_last_yield; -} - -uint16_t csi_collector_get_send_fail_count(void) -{ - uint32_t f = s_send_fail; - return (f > 0xFFFFu) ? 0xFFFFu : (uint16_t)f; + ESP_LOGI(TAG, "CSI collection initialized (node_id=%d, channel=%d)", + g_nvs_config.node_id, CONFIG_CSI_WIFI_CHANNEL); } /* ---- ADR-029: Channel hopping ---- */ diff --git a/firmware/esp32-csi-node/main/display_ui.c b/firmware/esp32-csi-node/main/display_ui.c index 901867fbd..2301b99fc 100644 --- a/firmware/esp32-csi-node/main/display_ui.c +++ b/firmware/esp32-csi-node/main/display_ui.c @@ -7,12 +7,8 @@ */ #include "display_ui.h" -#include "nvs_config.h" -#include "csi_collector.h" /* csi_collector_get_node_id() - defensive #390 */ #include "sdkconfig.h" -extern nvs_config_t g_nvs_config; - #if CONFIG_DISPLAY_ENABLE #include @@ -22,6 +18,7 @@ extern nvs_config_t g_nvs_config; #include "esp_timer.h" #include "esp_heap_caps.h" #include "edge_processing.h" +#include "nvs_config.h" static const char *TAG = "disp_ui"; @@ -351,7 +348,7 @@ void display_ui_update(void) { char buf[48]; - snprintf(buf, sizeof(buf), "Node: %u", (unsigned)csi_collector_get_node_id()); + snprintf(buf, sizeof(buf), "Node: %d", g_nvs_config.node_id); lv_label_set_text(s_sys_node, buf); snprintf(buf, sizeof(buf), "Heap: %lu KB free", diff --git a/firmware/esp32-csi-node/main/edge_processing.c b/firmware/esp32-csi-node/main/edge_processing.c index 94680e528..a14c877d9 100644 --- a/firmware/esp32-csi-node/main/edge_processing.c +++ b/firmware/esp32-csi-node/main/edge_processing.c @@ -18,14 +18,9 @@ */ #include "edge_processing.h" -#include "nvs_config.h" -#include "csi_collector.h" /* csi_collector_get_node_id() - defensive #390 */ -#include "mmwave_sensor.h" - -/* Runtime config — declared in main.c, loaded from NVS at boot. */ -extern nvs_config_t g_nvs_config; #include "wasm_runtime.h" #include "stream_sender.h" +#include "nvs_config.h" #include #include @@ -42,20 +37,12 @@ static const char *TAG = "edge_proc"; * ====================================================================== */ static edge_ring_buf_t s_ring; -static uint32_t s_ring_drops; /* Frames dropped due to full ring buffer. */ - -/* Scratch buffers for BPM estimation — moved from stack to static to avoid - * stack overflow. process_frame + update_multi_person_vitals combined used - * ~6.5-7.5 KB of the 8 KB task stack. These save ~4 KB of stack. */ -static float s_scratch_br[EDGE_PHASE_HISTORY_LEN]; -static float s_scratch_hr[EDGE_PHASE_HISTORY_LEN]; static inline bool ring_push(const uint8_t *iq, uint16_t len, int8_t rssi, uint8_t channel) { uint32_t next = (s_ring.head + 1) % EDGE_RING_SLOTS; if (next == s_ring.tail) { - s_ring_drops++; return false; /* Full — drop frame. */ } @@ -258,10 +245,6 @@ static uint32_t s_frame_count; /** Previous phase velocity for fall detection (acceleration). */ static float s_prev_phase_velocity; -/** Fall detection debounce state (issue #263). */ -static uint8_t s_fall_consec_count; /**< Consecutive frames above threshold. */ -static int64_t s_fall_last_alert_us; /**< Timestamp of last fall alert (debounce). */ - /** Adaptive calibration state. */ static bool s_calibrated; static float s_calib_sum; @@ -277,9 +260,6 @@ static uint8_t s_prev_iq[EDGE_MAX_IQ_BYTES]; static uint16_t s_prev_iq_len; static bool s_has_prev_iq; -/** ADR-069: Feature vector sequence counter. */ -static uint16_t s_feature_seq; - /** Multi-person vitals state. */ static edge_person_vitals_t s_persons[EDGE_MAX_PERSONS]; static edge_biquad_t s_person_bq_br[EDGE_MAX_PERSONS]; @@ -414,10 +394,10 @@ static uint16_t delta_compress(const uint8_t *curr, uint16_t len, } /** - * Send a compressed CSI frame (magic 0xC5110005, reassigned from 0xC5110003 for ADR-069). + * Send a compressed CSI frame (magic 0xC5110003). * * Header: - * [0..3] Magic 0xC5110005 (LE) + * [0..3] Magic 0xC5110003 (LE) * [4] Node ID * [5] Channel * [6..7] Original I/Q length (LE u16) @@ -442,7 +422,7 @@ static void send_compressed_frame(const uint8_t *iq_data, uint16_t iq_len, uint32_t magic = EDGE_COMPRESSED_MAGIC; memcpy(&pkt[0], &magic, 4); - pkt[4] = csi_collector_get_node_id(); /* #390: defensive copy */ + pkt[4] = g_nvs_config.node_id; pkt[5] = channel; memcpy(&pkt[6], &iq_len, 2); memcpy(&pkt[8], &comp_len, 2); @@ -523,18 +503,20 @@ static void update_multi_person_vitals(const uint8_t *iq_data, uint16_t n_sc, /* Estimate BPM when we have enough history. */ if (pv->history_len >= 64) { - /* Build contiguous buffer (reuse static scratch to save ~2 KB stack). */ + /* Build contiguous buffer for zero-crossing. */ + float br_buf[EDGE_PHASE_HISTORY_LEN]; + float hr_buf[EDGE_PHASE_HISTORY_LEN]; uint16_t buf_len = pv->history_len; for (uint16_t i = 0; i < buf_len; i++) { uint16_t ri = (pv->history_idx + EDGE_PHASE_HISTORY_LEN - buf_len + i) % EDGE_PHASE_HISTORY_LEN; - s_scratch_br[i] = s_person_br_filt[p][ri]; - s_scratch_hr[i] = s_person_hr_filt[p][ri]; + br_buf[i] = s_person_br_filt[p][ri]; + hr_buf[i] = s_person_hr_filt[p][ri]; } - float br = estimate_bpm_zero_crossing(s_scratch_br, buf_len, sample_rate); - float hr = estimate_bpm_zero_crossing(s_scratch_hr, buf_len, sample_rate); + float br = estimate_bpm_zero_crossing(br_buf, buf_len, sample_rate); + float hr = estimate_bpm_zero_crossing(hr_buf, buf_len, sample_rate); /* Sanity clamp. */ if (br >= 6.0f && br <= 40.0f) pv->breathing_bpm = br; @@ -558,7 +540,7 @@ static void send_vitals_packet(void) memset(&pkt, 0, sizeof(pkt)); pkt.magic = EDGE_VITALS_MAGIC; - pkt.node_id = csi_collector_get_node_id(); /* #390: defensive copy */ + pkt.node_id = g_nvs_config.node_id; pkt.flags = 0; if (s_presence_detected) pkt.flags |= 0x01; @@ -584,121 +566,7 @@ static void send_vitals_packet(void) s_latest_pkt = pkt; s_pkt_valid = true; - /* ADR-063: If mmWave is active, send fused 48-byte packet instead. */ - mmwave_state_t mw; - if (mmwave_sensor_get_state(&mw) && mw.detected) { - edge_fused_vitals_pkt_t fpkt; - memset(&fpkt, 0, sizeof(fpkt)); - - fpkt.magic = EDGE_FUSED_MAGIC; - fpkt.node_id = pkt.node_id; - fpkt.flags = pkt.flags; - if (mw.person_present) fpkt.flags |= 0x08; /* Bit3 = mmwave_present */ - fpkt.rssi = pkt.rssi; - fpkt.n_persons = pkt.n_persons; - fpkt.mmwave_type = (uint8_t)mw.type; - fpkt.motion_energy = pkt.motion_energy; - fpkt.presence_score = pkt.presence_score; - fpkt.timestamp_ms = pkt.timestamp_ms; - - /* Kalman-style fusion: prefer mmWave when available, CSI as fallback. */ - if (mw.heart_rate_bpm > 0.0f && s_heartrate_bpm > 0.0f) { - /* Weighted average: mmWave 80%, CSI 20% (mmWave is more accurate). */ - float fused_hr = mw.heart_rate_bpm * 0.8f + s_heartrate_bpm * 0.2f; - fpkt.heartrate = (uint32_t)(fused_hr * 10000.0f); - fpkt.fusion_confidence = 90; - } else if (mw.heart_rate_bpm > 0.0f) { - fpkt.heartrate = (uint32_t)(mw.heart_rate_bpm * 10000.0f); - fpkt.fusion_confidence = 85; - } else { - fpkt.heartrate = pkt.heartrate; - fpkt.fusion_confidence = 50; - } - - if (mw.breathing_rate > 0.0f && s_breathing_bpm > 0.0f) { - float fused_br = mw.breathing_rate * 0.8f + s_breathing_bpm * 0.2f; - fpkt.breathing_rate = (uint16_t)(fused_br * 100.0f); - } else if (mw.breathing_rate > 0.0f) { - fpkt.breathing_rate = (uint16_t)(mw.breathing_rate * 100.0f); - } else { - fpkt.breathing_rate = pkt.breathing_rate; - } - - /* Raw mmWave values for server-side analysis. */ - fpkt.mmwave_hr_bpm = mw.heart_rate_bpm; - fpkt.mmwave_br_bpm = mw.breathing_rate; - fpkt.mmwave_distance = mw.distance_cm; - fpkt.mmwave_targets = mw.target_count; - fpkt.mmwave_confidence = (mw.frame_count > 10) ? 80 : 40; - - stream_sender_send((const uint8_t *)&fpkt, sizeof(fpkt)); - } else { - /* No mmWave — send standard 32-byte packet. */ - stream_sender_send((const uint8_t *)&pkt, sizeof(pkt)); - } -} - -/* ====================================================================== - * ADR-069: Feature Vector Packet (48 bytes, sent at 1 Hz alongside vitals) - * ====================================================================== */ - -static void send_feature_vector(void) -{ - edge_feature_pkt_t pkt; - memset(&pkt, 0, sizeof(pkt)); - - pkt.magic = EDGE_FEATURE_MAGIC; - pkt.node_id = csi_collector_get_node_id(); /* #390: defensive copy */ - pkt.reserved = 0; - pkt.seq = s_feature_seq++; - pkt.timestamp_us = esp_timer_get_time(); - - /* Dim 0: Presence score (0.0-1.0, normalized from raw score) */ - float p = s_presence_score; - pkt.features[0] = p > 10.0f ? 1.0f : (p < 0.0f ? 0.0f : p / 10.0f); - - /* Dim 1: Motion energy (normalized, 0-1 range) */ - float m = s_motion_energy; - pkt.features[1] = m > 10.0f ? 1.0f : (m < 0.0f ? 0.0f : m / 10.0f); - - /* Dim 2: Breathing rate (BPM / 30, 0-1 range) */ - pkt.features[2] = s_breathing_bpm > 0.0f - ? (s_breathing_bpm / 30.0f > 1.0f ? 1.0f : s_breathing_bpm / 30.0f) - : 0.0f; - - /* Dim 3: Heart rate (BPM / 120, 0-1 range) */ - pkt.features[3] = s_heartrate_bpm > 0.0f - ? (s_heartrate_bpm / 120.0f > 1.0f ? 1.0f : s_heartrate_bpm / 120.0f) - : 0.0f; - - /* Dim 4: Phase variance mean (top-K subcarriers) */ - float var_mean = 0.0f; - if (s_top_k_count > 0) { - float var_sum = 0.0f; - uint8_t k = s_top_k_count < EDGE_TOP_K ? s_top_k_count : EDGE_TOP_K; - for (uint8_t i = 0; i < k; i++) { - var_sum += (float)welford_variance(&s_subcarrier_var[s_top_k[i]]); - } - var_mean = var_sum / (float)k; - } - pkt.features[4] = var_mean > 1.0f ? 1.0f : (var_mean < 0.0f ? 0.0f : var_mean); - - /* Dim 5: Person count (n_persons / 4, 0-1 range) */ - uint8_t n_active = 0; - for (uint8_t i = 0; i < EDGE_MAX_PERSONS; i++) { - if (s_persons[i].active) n_active++; - } - pkt.features[5] = (float)n_active / 4.0f; - if (pkt.features[5] > 1.0f) pkt.features[5] = 1.0f; - - /* Dim 6: Fall risk (0.0 or 1.0 based on recent detection) */ - pkt.features[6] = s_fall_detected ? 1.0f : 0.0f; - - /* Dim 7: RSSI normalized ((rssi + 100) / 100, 0-1 range) */ - pkt.features[7] = ((float)s_latest_rssi + 100.0f) / 100.0f; - if (pkt.features[7] > 1.0f) pkt.features[7] = 1.0f; - if (pkt.features[7] < 0.0f) pkt.features[7] = 0.0f; - + /* Send over UDP. */ stream_sender_send((const uint8_t *)&pkt, sizeof(pkt)); } @@ -714,11 +582,8 @@ static void process_frame(const edge_ring_slot_t *slot) s_frame_count++; s_latest_rssi = slot->rssi; - /* CSI sample rate. MGMT-only promiscuous filter (RuView#396, csi_collector.c) - * yields ~10 Hz from beacons; keep this value aligned with csi_collector's - * effective callback rate or estimate_bpm_zero_crossing() reports the wrong - * BPM (2× rate mismatch → 2× wrong breathing/HR). */ - const float sample_rate = 10.0f; + /* Assumed CSI sample rate (~20 Hz for typical ESP32 CSI). */ + const float sample_rate = 20.0f; /* --- Step 1-2: Phase extraction + unwrapping per subcarrier --- */ float phases[EDGE_MAX_SUBCARRIERS]; @@ -765,18 +630,20 @@ static void process_frame(const edge_ring_slot_t *slot) /* --- Step 7: BPM estimation (zero-crossing) --- */ if (s_history_len >= 64) { - /* Build contiguous buffers from ring (using static scratch to save stack). */ + /* Build contiguous buffers from ring. */ + float br_buf[EDGE_PHASE_HISTORY_LEN]; + float hr_buf[EDGE_PHASE_HISTORY_LEN]; uint16_t buf_len = s_history_len; for (uint16_t i = 0; i < buf_len; i++) { uint16_t ri = (s_history_idx + EDGE_PHASE_HISTORY_LEN - buf_len + i) % EDGE_PHASE_HISTORY_LEN; - s_scratch_br[i] = s_breathing_filtered[ri]; - s_scratch_hr[i] = s_heartrate_filtered[ri]; + br_buf[i] = s_breathing_filtered[ri]; + hr_buf[i] = s_heartrate_filtered[ri]; } - float br_bpm = estimate_bpm_zero_crossing(s_scratch_br, buf_len, sample_rate); - float hr_bpm = estimate_bpm_zero_crossing(s_scratch_hr, buf_len, sample_rate); + float br_bpm = estimate_bpm_zero_crossing(br_buf, buf_len, sample_rate); + float hr_bpm = estimate_bpm_zero_crossing(hr_buf, buf_len, sample_rate); /* Sanity clamp: breathing 6-40 BPM, heart rate 40-180 BPM. */ if (br_bpm >= 6.0f && br_bpm <= 40.0f) s_breathing_bpm = br_bpm; @@ -815,7 +682,7 @@ static void process_frame(const edge_ring_slot_t *slot) } s_presence_detected = (s_presence_score > threshold); - /* --- Step 10: Fall detection (phase acceleration + debounce, issue #263) --- */ + /* --- Step 10: Fall detection (phase acceleration) --- */ if (s_history_len >= 3) { uint16_t i0 = (s_history_idx + EDGE_PHASE_HISTORY_LEN - 1) % EDGE_PHASE_HISTORY_LEN; uint16_t i1 = (s_history_idx + EDGE_PHASE_HISTORY_LEN - 2) % EDGE_PHASE_HISTORY_LEN; @@ -823,26 +690,10 @@ static void process_frame(const edge_ring_slot_t *slot) float accel = fabsf(velocity - s_prev_phase_velocity); s_prev_phase_velocity = velocity; - if (accel > s_cfg.fall_thresh) { - s_fall_consec_count++; - } else { - s_fall_consec_count = 0; - } - - /* Require EDGE_FALL_CONSEC_MIN consecutive frames above threshold, - * plus a cooldown period to prevent alert storms. */ - int64_t now_us = esp_timer_get_time(); - int64_t cooldown_us = (int64_t)EDGE_FALL_COOLDOWN_MS * 1000; - if (s_fall_consec_count >= EDGE_FALL_CONSEC_MIN - && (now_us - s_fall_last_alert_us) >= cooldown_us) - { - s_fall_detected = true; - s_fall_last_alert_us = now_us; - s_fall_consec_count = 0; - ESP_LOGW(TAG, "Fall detected! accel=%.4f > thresh=%.4f (consec=%u)", - accel, s_cfg.fall_thresh, EDGE_FALL_CONSEC_MIN); - } else if (s_fall_consec_count == 0) { - s_fall_detected = false; + s_fall_detected = (accel > s_cfg.fall_thresh); + if (s_fall_detected) { + ESP_LOGW(TAG, "Fall detected! accel=%.4f > thresh=%.4f", + accel, s_cfg.fall_thresh); } } @@ -859,18 +710,16 @@ static void process_frame(const edge_ring_slot_t *slot) int64_t interval_us = (int64_t)s_cfg.vital_interval_ms * 1000; if ((now_us - s_last_vitals_send_us) >= interval_us) { send_vitals_packet(); - send_feature_vector(); /* ADR-069: 48-byte feature vector at same 1 Hz cadence. */ s_last_vitals_send_us = now_us; if ((s_frame_count % 200) == 0) { ESP_LOGI(TAG, "Vitals: br=%.1f hr=%.1f motion=%.4f pres=%s " - "fall=%s persons=%u frames=%lu drops=%lu", + "fall=%s persons=%u frames=%lu", s_breathing_bpm, s_heartrate_bpm, s_motion_energy, s_presence_detected ? "YES" : "no", s_fall_detected ? "YES" : "no", (unsigned)s_latest_pkt.n_persons, - (unsigned long)s_frame_count, - (unsigned long)s_ring_drops); + (unsigned long)s_frame_count); } } @@ -908,31 +757,12 @@ static void edge_task(void *arg) edge_ring_slot_t slot; - /* Maximum frames to process before a longer yield. On busy LANs - * (corporate networks, many APs), the ring buffer fills continuously. - * Without a batch limit the task processes frames back-to-back with - * only 1-tick yields, which on high frame rates can still starve - * IDLE1 enough to trip the 5-second task watchdog. See #266, #321. */ - while (1) { - uint8_t processed = 0; - - while (processed < EDGE_BATCH_LIMIT && ring_pop(&slot)) { + if (ring_pop(&slot)) { process_frame(&slot); - processed++; - /* 1-tick yield between frames within a batch. */ - vTaskDelay(1); - } - - if (processed > 0) { - /* Post-batch yield: ~20 ms so IDLE1 can run and feed the - * Core 1 watchdog even under sustained load. Uses pdMS_TO_TICKS - * for tick-rate independence (minimum 1 tick). */ - { TickType_t d = pdMS_TO_TICKS(20); vTaskDelay(d > 0 ? d : 1); } } else { - /* No frames available — sleep one full tick. - * NOTE: pdMS_TO_TICKS(5) == 0 at 100 Hz, which would busy-spin. */ - vTaskDelay(1); + /* No frames available — yield briefly. */ + vTaskDelay(pdMS_TO_TICKS(1)); } } } @@ -1013,8 +843,6 @@ esp_err_t edge_processing_init(const edge_config_t *cfg) s_latest_rssi = 0; s_frame_count = 0; s_prev_phase_velocity = 0.0f; - s_fall_consec_count = 0; - s_fall_last_alert_us = 0; s_last_vitals_send_us = 0; s_has_prev_iq = false; s_prev_iq_len = 0; diff --git a/firmware/esp32-csi-node/main/nvs_config.h b/firmware/esp32-csi-node/main/nvs_config.h index 225b9b893..953158b33 100644 --- a/firmware/esp32-csi-node/main/nvs_config.h +++ b/firmware/esp32-csi-node/main/nvs_config.h @@ -73,4 +73,7 @@ typedef struct { */ void nvs_config_load(nvs_config_t *cfg); +/** Global config instance (defined in main.c, loaded at startup). */ +extern nvs_config_t g_nvs_config; + #endif /* NVS_CONFIG_H */ diff --git a/firmware/esp32-csi-node/main/wasm_runtime.c b/firmware/esp32-csi-node/main/wasm_runtime.c index 8696be9fa..b41b74c72 100644 --- a/firmware/esp32-csi-node/main/wasm_runtime.c +++ b/firmware/esp32-csi-node/main/wasm_runtime.c @@ -12,15 +12,12 @@ #include "sdkconfig.h" #include "wasm_runtime.h" -#include "nvs_config.h" -#include "csi_collector.h" /* csi_collector_get_node_id() - defensive #390 */ - -extern nvs_config_t g_nvs_config; #if defined(CONFIG_WASM_ENABLE) && defined(WASM3_AVAILABLE) #include "rvf_parser.h" #include "stream_sender.h" +#include "nvs_config.h" #include #include @@ -384,7 +381,7 @@ static void send_wasm_output(uint8_t slot_id) memset(&pkt, 0, sizeof(pkt)); pkt.magic = WASM_OUTPUT_MAGIC; - pkt.node_id = csi_collector_get_node_id(); /* #390: defensive copy */ + pkt.node_id = g_nvs_config.node_id; pkt.module_id = slot_id; pkt.event_count = n_filtered; diff --git a/firmware/esp32-csi-node/provision.py b/firmware/esp32-csi-node/provision.py index d6a0e2f0a..8df5d3c0b 100644 --- a/firmware/esp32-csi-node/provision.py +++ b/firmware/esp32-csi-node/provision.py @@ -143,7 +143,7 @@ def generate_nvs_binary(csv_content, size): os.unlink(p) -def flash_nvs(port, baud, nvs_bin): +def flash_nvs(port, baud, nvs_bin, chip="esp32s3"): """Flash the NVS partition binary to the ESP32.""" with tempfile.NamedTemporaryFile(suffix=".bin", delete=False) as f: f.write(nvs_bin) @@ -152,7 +152,7 @@ def flash_nvs(port, baud, nvs_bin): try: cmd = [ sys.executable, "-m", "esptool", - "--chip", "esp32s3", + "--chip", chip, "--port", port, "--baud", str(baud), # Keep underscore form — ESP-IDF v5.4 bundles esptool 4.10.0 which only @@ -161,7 +161,7 @@ def flash_nvs(port, baud, nvs_bin): "write_flash", hex(NVS_PARTITION_OFFSET), bin_path, ] - print(f"Flashing NVS partition ({len(nvs_bin)} bytes) to {port}...") + print(f"Flashing NVS partition ({len(nvs_bin)} bytes) to {port} ({chip})...") subprocess.check_call(cmd) print("NVS provisioning complete!") finally: @@ -175,6 +175,8 @@ def main(): ) parser.add_argument("--port", required=True, help="Serial port (e.g. COM7, /dev/ttyUSB0)") parser.add_argument("--baud", type=int, default=460800, help="Flash baud rate (default: 460800)") + parser.add_argument("--chip", choices=["esp32s3", "esp32c6"], default="esp32s3", + help="ESP32 chip type (default: esp32s3)") parser.add_argument("--ssid", help="WiFi SSID") parser.add_argument("--password", help="WiFi password") parser.add_argument("--target-ip", help="Aggregator host IP (e.g. 192.168.1.20)") @@ -261,6 +263,7 @@ def main(): if args.tdm_slot is not None and args.tdm_slot >= args.tdm_total: parser.error(f"--tdm-slot ({args.tdm_slot}) must be less than --tdm-total ({args.tdm_total})") +<<<<<<< HEAD # ADR-060: Validate channel and MAC filter if args.channel is not None: if not ((1 <= args.channel <= 14) or (36 <= args.channel <= 177)): @@ -277,7 +280,7 @@ def main(): except ValueError: parser.error(f"--filter-mac contains invalid hex bytes: '{args.filter_mac}'") - print("Building NVS configuration:") + print(f"Building NVS configuration for {args.chip}:") if args.ssid: print(f" WiFi SSID: {args.ssid}") if args.password is not None: @@ -337,11 +340,11 @@ def main(): with open(out, "wb") as f: f.write(nvs_bin) print(f"NVS binary saved to {out} ({len(nvs_bin)} bytes)") - print(f"Flash manually: python -m esptool --chip esp32s3 --port {args.port} " - f"write-flash 0x9000 {out}") + print(f"Flash manually: python -m esptool --chip {args.chip} --port {args.port} " + f"write_flash 0x9000 {out}") return - flash_nvs(args.port, args.baud, nvs_bin) + flash_nvs(args.port, args.baud, nvs_bin, args.chip) if __name__ == "__main__": diff --git a/tools/csi_inspector.py b/tools/csi_inspector.py new file mode 100644 index 000000000..cbfe3181d --- /dev/null +++ b/tools/csi_inspector.py @@ -0,0 +1,333 @@ +#!/usr/bin/env python3 +""" +CSI Packet Inspector - Diagnostic tool for RuView ESP32 CSI data +Analyzes UDP packets on port 5005 and validates CSI frame integrity. +""" + +import socket +import struct +import time +import argparse +from collections import defaultdict +from datetime import datetime + +# Expected magic number for CSI frames +MAGIC_CSI = 0xC5110001 +MAGIC_VITALS = 0xC5110002 +MAGIC_WASM = 0xC5110004 + +class DeviceStats: + def __init__(self): + self.packet_count = 0 + self.last_seq = None + self.seq_gaps = 0 + self.rssi_values = [] + self.noise_values = [] + self.subcarrier_counts = set() + self.node_ids = set() + self.last_seen = None + self.errors = [] + self.amplitudes_sum = 0 + self.amplitude_samples = 0 + +def parse_csi_frame(data): + """Parse a CSI frame and return structured data.""" + if len(data) < 20: + return None, "Packet too short" + + magic = struct.unpack("= expected_len: + for k in range(min(n_pairs, (len(data) - iq_start) // 2)): + i_val = data[iq_start + k * 2] + q_val = data[iq_start + k * 2 + 1] + # Convert to signed + i_val = i_val if i_val < 128 else i_val - 256 + q_val = q_val if q_val < 128 else q_val - 256 + amp = (i_val * i_val + q_val * q_val) ** 0.5 + amplitudes.append(amp) + + return { + "type": "csi", + "magic": magic, + "node_id": node_id, + "n_antennas": n_antennas, + "n_subcarriers": n_subcarriers, + "freq_mhz": freq_mhz, + "sequence": sequence, + "rssi": rssi, + "rssi_raw": rssi_raw, + "noise": noise, + "noise_raw": noise_raw, + "amplitudes": amplitudes, + "payload_len": len(data) + }, None + +def validate_frame(frame, device_stats): + """Validate a CSI frame and return list of issues.""" + issues = [] + + if frame["type"] != "csi": + return issues + + # Check RSSI (should be negative, typically -30 to -90) + if frame["rssi"] >= 0: + issues.append(f"RSSI={frame['rssi']} (raw={frame['rssi_raw']}) - should be negative") + elif frame["rssi"] > -20: + issues.append(f"RSSI={frame['rssi']} - unusually strong") + elif frame["rssi"] < -95: + issues.append(f"RSSI={frame['rssi']} - very weak signal") + + # Check noise floor (should be around -90 to -100) + if frame["noise"] == 0: + issues.append(f"Noise=0 - likely uninitialized") + elif frame["noise"] > -70: + issues.append(f"Noise={frame['noise']} - unusually high") + + # Check subcarrier count + valid_subs = [52, 56, 64, 114, 128, 234, 242, 256] + if frame["n_subcarriers"] not in valid_subs: + issues.append(f"Subcarriers={frame['n_subcarriers']} - unusual count") + + # Check frequency + if frame["freq_mhz"] < 2400 or frame["freq_mhz"] > 5900: + if frame["freq_mhz"] != 0: # 0 might mean not set + issues.append(f"Freq={frame['freq_mhz']}MHz - invalid") + + # Check sequence gaps + if device_stats.last_seq is not None: + expected = (device_stats.last_seq + 1) & 0xFFFFFFFF + if frame["sequence"] != expected and frame["sequence"] != 0: + # Allow for wrapping and small gaps + gap = (frame["sequence"] - device_stats.last_seq) & 0xFFFFFFFF + if gap > 1 and gap < 1000: + issues.append(f"Seq gap: {gap} packets missed") + + # Check amplitude data + if len(frame["amplitudes"]) > 0: + avg_amp = sum(frame["amplitudes"]) / len(frame["amplitudes"]) + if avg_amp < 1: + issues.append(f"Avg amplitude={avg_amp:.2f} - very low") + elif avg_amp > 100: + issues.append(f"Avg amplitude={avg_amp:.2f} - very high") + + return issues + +def print_header(): + print("\n" + "=" * 80) + print(" RuView CSI Packet Inspector") + print(" Monitoring UDP port 5005 for ESP32 CSI frames") + print("=" * 80) + +def print_stats(devices, duration): + print("\n" + "-" * 80) + print(f" Statistics after {duration:.1f} seconds") + print("-" * 80) + + # Check for duplicate node IDs + node_id_map = defaultdict(list) + for ip, stats in devices.items(): + for nid in stats.node_ids: + node_id_map[nid].append(ip) + + duplicates = {nid: ips for nid, ips in node_id_map.items() if len(ips) > 1} + + if duplicates: + print("\n ⚠️ DUPLICATE NODE IDs DETECTED:") + for nid, ips in duplicates.items(): + print(f" Node {nid}: {', '.join(ips)}") + + print("\n Device Summary:") + print(" " + "-" * 76) + print(f" {'IP Address':<16} {'NodeID':<8} {'Pkts':<8} {'RSSI':<12} {'Subs':<10} {'Status'}") + print(" " + "-" * 76) + + for ip in sorted(devices.keys()): + stats = devices[ip] + node_str = ",".join(str(n) for n in sorted(stats.node_ids)) + + if stats.rssi_values: + avg_rssi = sum(stats.rssi_values) / len(stats.rssi_values) + rssi_str = f"{avg_rssi:.0f} dBm" + else: + rssi_str = "N/A" + + subs_str = ",".join(str(s) for s in sorted(stats.subcarrier_counts)) + + # Determine status + has_errors = len(stats.errors) > 0 + status = "⚠️ Issues" if has_errors else "✅ OK" + + print(f" {ip:<16} {node_str:<8} {stats.packet_count:<8} {rssi_str:<12} {subs_str:<10} {status}") + + print(" " + "-" * 76) + + # Print detailed errors + print("\n Detailed Issues:") + for ip in sorted(devices.keys()): + stats = devices[ip] + if stats.errors: + print(f"\n {ip}:") + # Group similar errors + error_counts = defaultdict(int) + for err in stats.errors: + error_counts[err] += 1 + for err, count in sorted(error_counts.items(), key=lambda x: -x[1])[:5]: + print(f" - {err} (x{count})") + +def main(): + parser = argparse.ArgumentParser(description="CSI Packet Inspector for RuView") + parser.add_argument("-d", "--duration", type=int, default=10, + help="Duration to capture in seconds (default: 10)") + parser.add_argument("-p", "--port", type=int, default=5005, + help="UDP port to monitor (default: 5005)") + parser.add_argument("-v", "--verbose", action="store_true", + help="Print each packet") + parser.add_argument("--continuous", action="store_true", + help="Run continuously, print stats every 10 seconds") + args = parser.parse_args() + + print_header() + + # Create UDP socket + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + + try: + sock.bind(("0.0.0.0", args.port)) + except OSError as e: + print(f"\n ERROR: Cannot bind to port {args.port}: {e}") + print(" The sensing server may be using this port.") + print(" Stop the server first: sudo systemctl stop ruview") + return 1 + + sock.settimeout(1.0) + + devices = defaultdict(DeviceStats) + start_time = time.time() + last_stats_time = start_time + packet_count = 0 + + print(f"\n Capturing for {args.duration} seconds..." if not args.continuous else "\n Running continuously (Ctrl+C to stop)...") + print() + + try: + while True: + elapsed = time.time() - start_time + + if not args.continuous and elapsed >= args.duration: + break + + # Print periodic stats in continuous mode + if args.continuous and time.time() - last_stats_time >= 10: + print_stats(devices, time.time() - start_time) + last_stats_time = time.time() + + try: + data, addr = sock.recvfrom(2048) + ip = addr[0] + + frame, error = parse_csi_frame(data) + + if error: + devices[ip].errors.append(error) + if args.verbose: + print(f" [{ip}] ERROR: {error}") + continue + + if frame["type"] != "csi": + if args.verbose: + print(f" [{ip}] {frame['type'].upper()} packet") + continue + + stats = devices[ip] + stats.packet_count += 1 + stats.node_ids.add(frame["node_id"]) + stats.rssi_values.append(frame["rssi"]) + stats.noise_values.append(frame["noise"]) + stats.subcarrier_counts.add(frame["n_subcarriers"]) + stats.last_seen = time.time() + + if frame["amplitudes"]: + stats.amplitudes_sum += sum(frame["amplitudes"]) + stats.amplitude_samples += len(frame["amplitudes"]) + + issues = validate_frame(frame, stats) + stats.errors.extend(issues) + stats.last_seq = frame["sequence"] + + packet_count += 1 + + if args.verbose: + status = "⚠️ " + issues[0] if issues else "✅" + print(f" [{ip}] Node={frame['node_id']} RSSI={frame['rssi']:4d} " + f"Subs={frame['n_subcarriers']:3d} Seq={frame['sequence']:10d} {status}") + else: + # Progress indicator + if packet_count % 20 == 0: + print(f"\r Captured {packet_count} packets from {len(devices)} devices...", end="", flush=True) + + except socket.timeout: + continue + + except KeyboardInterrupt: + print("\n\n Interrupted by user.") + + finally: + sock.close() + + # Print final stats + print_stats(devices, time.time() - start_time) + + # Summary + print("\n" + "=" * 80) + total_issues = sum(len(s.errors) for s in devices.values()) + if total_issues == 0: + print(" ✅ All packets valid - no issues detected") + else: + print(f" ⚠️ Found {total_issues} issues across {len(devices)} devices") + print("\n Recommendations:") + + # Check specific issues + has_rssi_issue = any("RSSI" in e for s in devices.values() for e in s.errors) + has_dup_nodes = len({nid for s in devices.values() for nid in s.node_ids}) < len(devices) + has_noise_issue = any("Noise" in e for s in devices.values() for e in s.errors) + + if has_rssi_issue: + print(" 1. RSSI values incorrect - check ESP32 firmware byte encoding") + if has_dup_nodes: + print(" 2. Duplicate Node IDs - reflash each ESP32 with unique ID (1,2,3,4)") + if has_noise_issue: + print(" 3. Noise floor not set - update ESP32 firmware to include noise value") + + print("=" * 80 + "\n") + return 0 + +if __name__ == "__main__": + exit(main()) diff --git a/ui/observatory/css/observatory.css b/ui/observatory/css/observatory.css index e289d65ff..d21765ef7 100644 --- a/ui/observatory/css/observatory.css +++ b/ui/observatory/css/observatory.css @@ -107,11 +107,16 @@ body { } .dot--demo { background: var(--amber); box-shadow: 0 0 6px var(--amber); } .dot--live { background: var(--green-glow); box-shadow: 0 0 6px var(--green-glow); animation: pulse-dot 2s infinite; } +.dot--reconnecting { background: var(--blue-signal); box-shadow: 0 0 6px var(--blue-signal); animation: pulse-reconnect 0.8s infinite; } @keyframes pulse-dot { 0%, 100% { opacity: 1; } 50% { opacity: 0.4; } } +@keyframes pulse-reconnect { + 0%, 100% { opacity: 0.4; } + 50% { opacity: 1; } +} #scenario-area { display: flex; diff --git a/ui/observatory/js/hud-controller.js b/ui/observatory/js/hud-controller.js index 14afc6b54..cb7f62a7e 100644 --- a/ui/observatory/js/hud-controller.js +++ b/ui/observatory/js/hud-controller.js @@ -362,7 +362,9 @@ export class HudController { updateSourceBadge(dataSource, ws) { const dot = document.querySelector('#data-source-badge .dot'); const label = document.getElementById('data-source-label'); - if (dataSource === 'ws' && ws?.readyState === WebSocket.OPEN) { + if (dataSource === 'reconnecting') { + dot.className = 'dot dot--reconnecting'; label.textContent = 'RECONNECTING'; + } else if (dataSource === 'ws' && ws?.readyState === WebSocket.OPEN) { dot.className = 'dot dot--live'; label.textContent = 'LIVE'; } else { dot.className = 'dot dot--demo'; label.textContent = 'DEMO'; @@ -421,8 +423,9 @@ export class HudController { this._setText('var-value', (feat.variance || 0).toFixed(2)); this._setText('motion-value', (feat.motion_band_power || 0).toFixed(3)); - // Mini person-count dots - const personCount = data.estimated_persons || 0; + // Mini person-count dots (server returns 0-3, clamp to valid range) + const rawCount = data.estimated_persons; + const personCount = (typeof rawCount === 'number' && rawCount >= 0 && rawCount <= 8) ? rawCount : 0; this._updatePersonDots(personCount); const presEl = document.getElementById('presence-indicator'); diff --git a/ui/observatory/js/main.js b/ui/observatory/js/main.js index 26abbe2c7..ba69456dd 100644 --- a/ui/observatory/js/main.js +++ b/ui/observatory/js/main.js @@ -130,6 +130,10 @@ class Observatory { // WebSocket for live data — always try auto-detect on startup this._ws = null; this._liveData = null; + this._wsUrl = null; // Store URL for reconnection + this._wsReconnectAttempts = 0; + this._wsMaxReconnectAttempts = 5; + this._wsReconnectTimer = null; this._autoDetectLive(); // Input @@ -474,24 +478,72 @@ class Observatory { _connectWS(url) { this._disconnectWS(); + this._wsUrl = url; // Store for reconnection try { this._ws = new WebSocket(url); this._ws.onopen = () => { console.log('[Observatory] WebSocket connected'); + this._wsReconnectAttempts = 0; // Reset on successful connection this._hud.updateSourceBadge('ws', this._ws); }; - this._ws.onmessage = (evt) => { try { this._liveData = JSON.parse(evt.data); } catch {} }; + this._ws.onmessage = (evt) => { + try { + const msg = JSON.parse(evt.data); + // Only use SensingUpdate messages (have timestamp + features + classification). + // Ignore edge_vitals, wasm_event, and other message types. + if (msg.type === 'edge_vitals' || msg.type === 'wasm_event' || msg.type === 'pose_data') { + return; // Skip non-SensingUpdate messages + } + // SensingUpdate should have timestamp, features, and classification + if (msg.timestamp && msg.features && msg.classification) { + this._liveData = msg; + } + } catch {} + }; this._ws.onclose = () => { - console.log('[Observatory] WebSocket closed, falling back to demo'); this._ws = null; - this.settings.dataSource = 'demo'; - this._hud.updateSourceBadge('demo', null); + this._scheduleReconnect(); }; this._ws.onerror = () => {}; } catch {} } + _scheduleReconnect() { + if (this._wsReconnectTimer) { + clearTimeout(this._wsReconnectTimer); + } + + this._wsReconnectAttempts++; + + if (this._wsReconnectAttempts > this._wsMaxReconnectAttempts) { + console.log('[Observatory] Max reconnect attempts reached, falling back to demo'); + this.settings.dataSource = 'demo'; + this._hud.updateSourceBadge('demo', null); + return; + } + + // Exponential backoff: 1s, 2s, 4s, 8s, 16s + const delay = Math.min(1000 * Math.pow(2, this._wsReconnectAttempts - 1), 16000); + console.log(`[Observatory] WebSocket closed, reconnecting in ${delay/1000}s (attempt ${this._wsReconnectAttempts}/${this._wsMaxReconnectAttempts})`); + + // Show reconnecting status in the HUD + this._hud.updateSourceBadge('reconnecting', null); + + this._wsReconnectTimer = setTimeout(() => { + if (this._wsUrl) { + console.log('[Observatory] Attempting to reconnect...'); + this._connectWS(this._wsUrl); + } + }, delay); + } + _disconnectWS() { + // Clear any pending reconnect timer + if (this._wsReconnectTimer) { + clearTimeout(this._wsReconnectTimer); + this._wsReconnectTimer = null; + } + this._wsReconnectAttempts = 0; if (this._ws) { this._ws.close(); this._ws = null; } this._liveData = null; } diff --git a/v2/crates/wifi-densepose-sensing-server/src/main.rs b/v2/crates/wifi-densepose-sensing-server/src/main.rs index a8b207e47..c60a716f4 100644 --- a/v2/crates/wifi-densepose-sensing-server/src/main.rs +++ b/v2/crates/wifi-densepose-sensing-server/src/main.rs @@ -177,8 +177,8 @@ struct Esp32Frame { magic: u32, node_id: u8, n_antennas: u8, - n_subcarriers: u8, - freq_mhz: u16, + n_subcarriers: u16, // ADR-018: u16 at bytes [6..7] + freq_mhz: u32, // ADR-018: u32 at bytes [8..11] sequence: u32, rssi: i8, noise_floor: i8, @@ -290,6 +290,14 @@ struct PersonDetection { keypoints: Vec, bbox: BoundingBox, zone: String, + /// Pose type for Observatory skeleton visualization ('standing', 'walking', 'sitting', etc.) + pose: String, + /// 3D world position [x, y, z] for Observatory + position: [f64; 3], + /// Motion intensity score (0-100+) for animation + motion_score: f64, + /// Facing direction in radians for skeleton rotation + facing: f64, } #[derive(Debug, Clone, Serialize, Deserialize)] @@ -824,13 +832,12 @@ fn parse_esp32_frame(buf: &[u8]) -> Option { // [20..] I/Q data let node_id = buf[4]; let n_antennas = buf[5]; - let n_subcarriers = buf[6]; - let freq_mhz = u16::from_le_bytes([buf[8], buf[9]]); - let sequence = u32::from_le_bytes([buf[10], buf[11], buf[12], buf[13]]); - let rssi_raw = buf[14] as i8; - // Fix RSSI sign: ensure it's always negative (dBm convention). - let rssi = if rssi_raw > 0 { rssi_raw.saturating_neg() } else { rssi_raw }; - let noise_floor = buf[15] as i8; + // ADR-018 20-byte header: n_subcarriers is u16 at [6..7], freq is u32 at [8..11] + let n_subcarriers = u16::from_le_bytes([buf[6], buf[7]]); + let freq_mhz = u32::from_le_bytes([buf[8], buf[9], buf[10], buf[11]]); + let sequence = u32::from_le_bytes([buf[12], buf[13], buf[14], buf[15]]); + let rssi = buf[16] as i8; + let noise_floor = buf[17] as i8; let iq_start = 20; let n_pairs = n_antennas as usize * n_subcarriers as usize; @@ -1277,9 +1284,9 @@ fn raw_classify(score: f64) -> String { } /// Debounce frames required before state transition (at ~10 FPS = ~0.4s). -const DEBOUNCE_FRAMES: u32 = 4; +const DEBOUNCE_FRAMES: u32 = 2; // Faster state transitions /// EMA alpha for motion smoothing (~1s time constant at 10 FPS). -const MOTION_EMA_ALPHA: f64 = 0.15; +const MOTION_EMA_ALPHA: f64 = 0.25; // More responsive to motion changes /// EMA alpha for slow-adapting baseline (~30s time constant at 10 FPS). const BASELINE_EMA_ALPHA: f64 = 0.003; /// Number of warm-up frames before baseline subtraction kicks in. @@ -1661,7 +1668,7 @@ async fn windows_wifi_task(state: SharedState, tick_ms: u64) { magic: 0xC511_0001, node_id: 0, n_antennas: 1, - n_subcarriers: obs_count.min(255) as u8, + n_subcarriers: obs_count.min(65535) as u16, freq_mhz: 2437, sequence: seq, rssi: first_rssi.clamp(-128.0, 127.0) as i8, @@ -1728,9 +1735,9 @@ async fn windows_wifi_task(state: SharedState, tick_ms: u64) { let feat_variance = features.variance; - // Multi-person estimation with temporal smoothing (EMA α=0.10). + // Multi-person estimation with asymmetric smoothing (faster decay when leaving). let raw_score = compute_person_score(&features); - s.smoothed_person_score = s.smoothed_person_score * 0.90 + raw_score * 0.10; + s.smoothed_person_score = smooth_person_score(s.smoothed_person_score, raw_score); let est_persons = if classification.presence { let count = s.person_count(); s.prev_person_count = count; @@ -1867,9 +1874,9 @@ async fn windows_wifi_fallback_tick(state: &SharedState, seq: u32) { let feat_variance = features.variance; - // Multi-person estimation with temporal smoothing (EMA α=0.10). + // Multi-person estimation with asymmetric smoothing (faster decay when leaving). let raw_score = compute_person_score(&features); - s.smoothed_person_score = s.smoothed_person_score * 0.90 + raw_score * 0.10; + s.smoothed_person_score = smooth_person_score(s.smoothed_person_score, raw_score); let est_persons = if classification.presence { let count = s.person_count(); s.prev_person_count = count; @@ -1976,7 +1983,7 @@ fn generate_simulated_frame(tick: u64) -> Esp32Frame { magic: 0xC511_0001, node_id: 1, n_antennas: 1, - n_subcarriers: n_sub as u8, + n_subcarriers: n_sub as u16, freq_mhz: 2437, sequence: tick as u32, rssi: (-40.0 + 5.0 * (t * 0.2).sin()) as i8, @@ -2094,6 +2101,10 @@ async fn handle_ws_pose_client(mut socket: WebSocket, state: SharedState) { bbox: BoundingBox { x: 260.0, y: 150.0, width: 120.0, height: 220.0 }, keypoints, zone: "zone_1".into(), + pose: "standing".to_string(), + position: [0.0, 0.0, 0.0], + motion_score: sensing.features.motion_band_power.min(100.0), + facing: 0.0, }] }).unwrap_or_else(|| { // Prefer tracked persons from broadcast if available @@ -2435,6 +2446,15 @@ fn score_to_person_count(smoothed_score: f64, prev_count: usize) -> usize { } } +/// Asymmetric EMA for person score: faster decay when someone leaves, moderate rise. +/// +/// When raw_score < current (activity dropping = someone left), use α=0.35 for faster response. +/// When raw_score > current (activity rising = someone entered), use α=0.20 for balanced responsiveness. +fn smooth_person_score(current: f64, raw: f64) -> f64 { + let alpha = if raw < current { 0.35 } else { 0.20 }; + current * (1.0 - alpha) + raw * alpha +} + /// Generate a single person's skeleton with per-person spatial offset and phase stagger. /// /// `person_idx`: 0-based index of this person. @@ -2601,6 +2621,37 @@ fn derive_single_person_pose( let max_x = xs.iter().cloned().fold(f64::MIN, f64::max) + 10.0; let max_y = ys.iter().cloned().fold(f64::MIN, f64::max) + 10.0; + // Derive pose type from motion level and posture + let pose_type = if let Some(ref posture) = update.posture { + match posture.as_str() { + "lying" | "fallen" => "lying", + "sitting" => "sitting", + _ if is_walking => "walking", + _ => "standing", + } + } else if is_walking { + "walking" + } else { + "standing" + }; + + // Convert 2D pixel position to 3D world coordinates for Observatory + // Observatory uses: X = left/right, Y = up (always 0 for ground), Z = forward/back + // Map pixel X (0-640) to world X (-5 to 5), pixel Y to world Z + let world_x = (base_x - 320.0) / 64.0; // Center at 0, scale to ~±5 meters + let world_z = (base_y - 240.0) / 48.0; // Map Y to Z depth + + // Per-person spatial offset in world coordinates + let half_w = (total_persons as f64 - 1.0) / 2.0; + let person_world_x = world_x + (person_idx as f64 - half_w) * 1.5; // 1.5m spacing + + // Facing direction based on motion (walking direction) + let facing = if is_walking { + stride_x.signum() * 0.3 // Face movement direction + } else { + 0.0 + }; + PersonDetection { id: (person_idx + 1) as u32, confidence: cls.confidence * conf_decay, @@ -2612,6 +2663,10 @@ fn derive_single_person_pose( height: (max_y - min_y).max(160.0), }, zone: format!("zone_{}", person_idx + 1), + pose: pose_type.to_string(), + position: [person_world_x, 0.0, world_z], + motion_score: motion_score * 100.0, // Scale to 0-100 range + facing, } } @@ -3979,12 +4034,13 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) { match fused { Some(ref f) => { let score = multistatic_bridge::compute_person_score_from_amplitudes(&f.fused_amplitude); - s.smoothed_person_score = s.smoothed_person_score * 0.90 + score * 0.10; + // Asymmetric smoothing: faster decay when leaving. + s.smoothed_person_score = smooth_person_score(s.smoothed_person_score, score); let count = s.person_count(); s.prev_person_count = count; - count.max(1) + count } - None => fallback_count.unwrap_or(0).max(1), + None => fallback_count.unwrap_or(0), } } else { s.prev_person_count = 0; @@ -4126,9 +4182,9 @@ async fn simulated_data_task(state: SharedState, tick_ms: u64) { let frame_amplitudes = frame.amplitudes.clone(); let frame_n_sub = frame.n_subcarriers; - // Multi-person estimation with temporal smoothing (EMA α=0.10). + // Multi-person estimation with asymmetric smoothing (faster decay when leaving). let raw_score = compute_person_score(&features); - s.smoothed_person_score = s.smoothed_person_score * 0.90 + raw_score * 0.10; + s.smoothed_person_score = smooth_person_score(s.smoothed_person_score, raw_score); let est_persons = if classification.presence { let count = s.person_count(); s.prev_person_count = count;