feat(firmware): ADR-063 mmWave sensor fusion — full implementation
Phase 1-2 of ADR-063: mmwave_sensor.c/h: - MR60BHA2 UART parser (60 GHz: HR, BR, presence, distance) - LD2410 UART parser (24 GHz: presence, distance) - Auto-detection: probes UART for known frame headers at boot - Mock generator for QEMU testing (synthetic HR 72±2, BR 16±1) - Capability flag registration per sensor type edge_processing.c/h: - 48-byte fused vitals packet (magic 0xC5110004) - Kalman-style fusion: mmWave 80% + CSI 20% when both available - Automatic fallback to CSI-only 32-byte packet when no mmWave - Dual presence flag (Bit3 = mmwave_present) main.c: - mmwave_sensor_init() called at boot with auto-detect - Status logged in startup banner Fuzz stubs updated for mmwave_sensor API. Build verified: QEMU mock build passes. Co-Authored-By: claude-flow <ruv@ruv.net>
This commit is contained in:
ruv
parent
4a50136365
commit
f42df4afaa
|
|
@ -2,6 +2,7 @@ set(SRCS
|
|||
"main.c" "csi_collector.c" "stream_sender.c" "nvs_config.c"
|
||||
"edge_processing.c" "ota_update.c" "power_mgmt.c"
|
||||
"wasm_runtime.c" "wasm_upload.c" "rvf_parser.c"
|
||||
"mmwave_sensor.c"
|
||||
)
|
||||
|
||||
set(REQUIRES "")
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
*/
|
||||
|
||||
#include "edge_processing.h"
|
||||
#include "mmwave_sensor.h"
|
||||
#include "wasm_runtime.h"
|
||||
#include "stream_sender.h"
|
||||
|
||||
|
|
@ -577,8 +578,58 @@ static void send_vitals_packet(void)
|
|||
s_latest_pkt = pkt;
|
||||
s_pkt_valid = true;
|
||||
|
||||
/* Send over UDP. */
|
||||
stream_sender_send((const uint8_t *)&pkt, sizeof(pkt));
|
||||
/* 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));
|
||||
}
|
||||
}
|
||||
|
||||
/* ======================================================================
|
||||
|
|
|
|||
|
|
@ -106,6 +106,35 @@ typedef struct __attribute__((packed)) {
|
|||
|
||||
_Static_assert(sizeof(edge_vitals_pkt_t) == 32, "vitals packet must be 32 bytes");
|
||||
|
||||
/* ---- ADR-063: Fused vitals packet (48 bytes, wire format) ---- */
|
||||
#define EDGE_FUSED_MAGIC 0xC5110004 /**< Fused vitals packet magic. */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
/* First 32 bytes match edge_vitals_pkt_t layout */
|
||||
uint32_t magic; /**< EDGE_FUSED_MAGIC = 0xC5110004. */
|
||||
uint8_t node_id;
|
||||
uint8_t flags; /**< Bit0=presence, Bit1=fall, Bit2=motion, Bit3=mmwave_present. */
|
||||
uint16_t breathing_rate; /**< Fused BPM * 100 (CSI + mmWave Kalman). */
|
||||
uint32_t heartrate; /**< Fused BPM * 10000. */
|
||||
int8_t rssi;
|
||||
uint8_t n_persons;
|
||||
uint8_t mmwave_type; /**< mmwave_type_t enum. */
|
||||
uint8_t fusion_confidence; /**< 0-100 fusion quality score. */
|
||||
float motion_energy;
|
||||
float presence_score;
|
||||
uint32_t timestamp_ms;
|
||||
/* mmWave extension (16 bytes) */
|
||||
float mmwave_hr_bpm; /**< Raw mmWave heart rate. */
|
||||
float mmwave_br_bpm; /**< Raw mmWave breathing rate. */
|
||||
float mmwave_distance;/**< Distance to nearest target (cm). */
|
||||
uint8_t mmwave_targets; /**< Target count from mmWave. */
|
||||
uint8_t mmwave_confidence; /**< mmWave signal quality 0-100. */
|
||||
uint16_t reserved3;
|
||||
uint32_t reserved4; /**< Pad to 48 bytes for alignment. */
|
||||
} edge_fused_vitals_pkt_t;
|
||||
|
||||
_Static_assert(sizeof(edge_fused_vitals_pkt_t) == 48, "fused vitals must be 48 bytes");
|
||||
|
||||
/* ---- Edge configuration (from NVS) ---- */
|
||||
typedef struct {
|
||||
uint8_t tier; /**< Processing tier: 0=raw, 1=basic, 2=full. */
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include "wasm_runtime.h"
|
||||
#include "wasm_upload.h"
|
||||
#include "display_task.h"
|
||||
#include "mmwave_sensor.h"
|
||||
#ifdef CONFIG_CSI_MOCK_ENABLED
|
||||
#include "mock_csi.h"
|
||||
#endif
|
||||
|
|
@ -227,6 +228,18 @@ void app_main(void)
|
|||
}
|
||||
}
|
||||
|
||||
/* ADR-063: Initialize mmWave sensor (auto-detect on UART). */
|
||||
esp_err_t mmwave_ret = mmwave_sensor_init(-1, -1); /* -1 = use default GPIO pins */
|
||||
if (mmwave_ret == ESP_OK) {
|
||||
mmwave_state_t mw;
|
||||
if (mmwave_sensor_get_state(&mw)) {
|
||||
ESP_LOGI(TAG, "mmWave sensor: %s (caps=0x%04x)",
|
||||
mmwave_type_name(mw.type), mw.capabilities);
|
||||
}
|
||||
} else {
|
||||
ESP_LOGI(TAG, "No mmWave sensor detected (CSI-only mode)");
|
||||
}
|
||||
|
||||
/* Initialize power management. */
|
||||
power_mgmt_init(g_nvs_config.power_duty);
|
||||
|
||||
|
|
@ -238,11 +251,12 @@ void app_main(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
ESP_LOGI(TAG, "CSI streaming active → %s:%d (edge_tier=%u, OTA=%s, WASM=%s)",
|
||||
ESP_LOGI(TAG, "CSI streaming active → %s:%d (edge_tier=%u, OTA=%s, WASM=%s, mmWave=%s)",
|
||||
g_nvs_config.target_ip, g_nvs_config.target_port,
|
||||
g_nvs_config.edge_tier,
|
||||
(ota_ret == ESP_OK) ? "ready" : "off",
|
||||
(wasm_ret == ESP_OK) ? "ready" : "off");
|
||||
(wasm_ret == ESP_OK) ? "ready" : "off",
|
||||
(mmwave_ret == ESP_OK) ? "active" : "off");
|
||||
|
||||
/* Main loop — keep alive */
|
||||
while (1) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,533 @@
|
|||
/**
|
||||
* @file mmwave_sensor.c
|
||||
* @brief ADR-063: mmWave sensor UART driver with auto-detection.
|
||||
*
|
||||
* Supports Seeed MR60BHA2 (60 GHz) and HLK-LD2410 (24 GHz).
|
||||
* Under QEMU (CONFIG_CSI_MOCK_ENABLED), uses a mock generator
|
||||
* that produces synthetic vital signs for pipeline testing.
|
||||
*
|
||||
* MR60BHA2 frame format (Seeed proprietary):
|
||||
* Header: 0x53 0x59 (2 bytes)
|
||||
* Control: type_h type_l (2 bytes)
|
||||
* Length: len_h len_l (2 bytes, big-endian)
|
||||
* Data: [length bytes]
|
||||
* Footer: 0x54 0x43 (2 bytes)
|
||||
*
|
||||
* Type 0x01 0x01 = Breathing data
|
||||
* Type 0x02 0x01 = Heart rate data
|
||||
* Type 0x80 0x01 = Presence/distance
|
||||
*
|
||||
* LD2410 frame format (HLK binary):
|
||||
* Header: 0xF4 0xF3 0xF2 0xF1 (4 bytes)
|
||||
* Length: len_l len_h (2 bytes, little-endian)
|
||||
* Data: [length bytes, includes type byte]
|
||||
* Footer: 0xF8 0xF7 0xF6 0xF5 (4 bytes)
|
||||
*/
|
||||
|
||||
#include "mmwave_sensor.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "sdkconfig.h"
|
||||
|
||||
#ifndef CONFIG_CSI_MOCK_ENABLED
|
||||
#include "driver/uart.h"
|
||||
#endif
|
||||
|
||||
static const char *TAG = "mmwave";
|
||||
|
||||
/* ---- Configuration ---- */
|
||||
#define MMWAVE_UART_NUM UART_NUM_1
|
||||
#define MMWAVE_UART_BAUD 115200
|
||||
#define MMWAVE_BUF_SIZE 256
|
||||
#define MMWAVE_TASK_STACK 4096
|
||||
#define MMWAVE_TASK_PRIORITY 3
|
||||
#define MMWAVE_PROBE_TIMEOUT_MS 2000
|
||||
|
||||
/* ---- MR60BHA2 protocol constants ---- */
|
||||
#define MR60_HEADER_H 0x53
|
||||
#define MR60_HEADER_L 0x59
|
||||
#define MR60_FOOTER_H 0x54
|
||||
#define MR60_FOOTER_L 0x43
|
||||
|
||||
/* MR60BHA2 message types (type_h << 8 | type_l) */
|
||||
#define MR60_TYPE_BREATHING 0x0101
|
||||
#define MR60_TYPE_HEARTRATE 0x0201
|
||||
#define MR60_TYPE_PRESENCE 0x8001
|
||||
|
||||
/* ---- LD2410 protocol constants ---- */
|
||||
#define LD2410_HEADER { 0xF4, 0xF3, 0xF2, 0xF1 }
|
||||
#define LD2410_FOOTER { 0xF8, 0xF7, 0xF6, 0xF5 }
|
||||
|
||||
/* ---- Shared state ---- */
|
||||
static mmwave_state_t s_state;
|
||||
static volatile bool s_running;
|
||||
|
||||
/* ======================================================================
|
||||
* MR60BHA2 Parser
|
||||
* ====================================================================== */
|
||||
|
||||
typedef enum {
|
||||
MR60_WAIT_HEADER_H,
|
||||
MR60_WAIT_HEADER_L,
|
||||
MR60_READ_TYPE_H,
|
||||
MR60_READ_TYPE_L,
|
||||
MR60_READ_LEN_H,
|
||||
MR60_READ_LEN_L,
|
||||
MR60_READ_DATA,
|
||||
MR60_WAIT_FOOTER_H,
|
||||
MR60_WAIT_FOOTER_L,
|
||||
} mr60_parse_state_t;
|
||||
|
||||
typedef struct {
|
||||
mr60_parse_state_t state;
|
||||
uint8_t type_h, type_l;
|
||||
uint16_t data_len;
|
||||
uint16_t data_idx;
|
||||
uint8_t data[MMWAVE_BUF_SIZE];
|
||||
} mr60_parser_t;
|
||||
|
||||
static mr60_parser_t s_mr60;
|
||||
|
||||
static void mr60_process_frame(uint16_t type, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
s_state.frame_count++;
|
||||
s_state.last_update_us = esp_timer_get_time();
|
||||
|
||||
switch (type) {
|
||||
case MR60_TYPE_BREATHING:
|
||||
if (len >= 4) {
|
||||
/* Breathing rate as float32 (little-endian). */
|
||||
float br;
|
||||
memcpy(&br, data, sizeof(float));
|
||||
if (br >= 0.0f && br <= 60.0f) {
|
||||
s_state.breathing_rate = br;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MR60_TYPE_HEARTRATE:
|
||||
if (len >= 4) {
|
||||
float hr;
|
||||
memcpy(&hr, data, sizeof(float));
|
||||
if (hr >= 0.0f && hr <= 250.0f) {
|
||||
s_state.heart_rate_bpm = hr;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MR60_TYPE_PRESENCE:
|
||||
if (len >= 1) {
|
||||
s_state.person_present = (data[0] != 0);
|
||||
if (len >= 5) {
|
||||
float dist;
|
||||
memcpy(&dist, &data[1], sizeof(float));
|
||||
s_state.distance_cm = dist;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
/* Unknown frame type — ignore. */
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void mr60_feed_byte(uint8_t b)
|
||||
{
|
||||
switch (s_mr60.state) {
|
||||
case MR60_WAIT_HEADER_H:
|
||||
if (b == MR60_HEADER_H) s_mr60.state = MR60_WAIT_HEADER_L;
|
||||
break;
|
||||
case MR60_WAIT_HEADER_L:
|
||||
if (b == MR60_HEADER_L) s_mr60.state = MR60_READ_TYPE_H;
|
||||
else s_mr60.state = MR60_WAIT_HEADER_H;
|
||||
break;
|
||||
case MR60_READ_TYPE_H:
|
||||
s_mr60.type_h = b;
|
||||
s_mr60.state = MR60_READ_TYPE_L;
|
||||
break;
|
||||
case MR60_READ_TYPE_L:
|
||||
s_mr60.type_l = b;
|
||||
s_mr60.state = MR60_READ_LEN_H;
|
||||
break;
|
||||
case MR60_READ_LEN_H:
|
||||
s_mr60.data_len = (uint16_t)b << 8;
|
||||
s_mr60.state = MR60_READ_LEN_L;
|
||||
break;
|
||||
case MR60_READ_LEN_L:
|
||||
s_mr60.data_len |= b;
|
||||
s_mr60.data_idx = 0;
|
||||
if (s_mr60.data_len == 0) {
|
||||
s_mr60.state = MR60_WAIT_FOOTER_H;
|
||||
} else if (s_mr60.data_len > MMWAVE_BUF_SIZE) {
|
||||
s_state.error_count++;
|
||||
s_mr60.state = MR60_WAIT_HEADER_H;
|
||||
} else {
|
||||
s_mr60.state = MR60_READ_DATA;
|
||||
}
|
||||
break;
|
||||
case MR60_READ_DATA:
|
||||
s_mr60.data[s_mr60.data_idx++] = b;
|
||||
if (s_mr60.data_idx >= s_mr60.data_len) {
|
||||
s_mr60.state = MR60_WAIT_FOOTER_H;
|
||||
}
|
||||
break;
|
||||
case MR60_WAIT_FOOTER_H:
|
||||
if (b == MR60_FOOTER_H) {
|
||||
s_mr60.state = MR60_WAIT_FOOTER_L;
|
||||
} else {
|
||||
s_state.error_count++;
|
||||
s_mr60.state = MR60_WAIT_HEADER_H;
|
||||
}
|
||||
break;
|
||||
case MR60_WAIT_FOOTER_L:
|
||||
if (b == MR60_FOOTER_L) {
|
||||
uint16_t type = ((uint16_t)s_mr60.type_h << 8) | s_mr60.type_l;
|
||||
mr60_process_frame(type, s_mr60.data, s_mr60.data_len);
|
||||
} else {
|
||||
s_state.error_count++;
|
||||
}
|
||||
s_mr60.state = MR60_WAIT_HEADER_H;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ======================================================================
|
||||
* LD2410 Parser
|
||||
* ====================================================================== */
|
||||
|
||||
typedef enum {
|
||||
LD_WAIT_F4, LD_WAIT_F3, LD_WAIT_F2, LD_WAIT_F1,
|
||||
LD_READ_LEN_L, LD_READ_LEN_H,
|
||||
LD_READ_DATA,
|
||||
LD_WAIT_F8, LD_WAIT_F7, LD_WAIT_F6, LD_WAIT_F5,
|
||||
} ld2410_parse_state_t;
|
||||
|
||||
typedef struct {
|
||||
ld2410_parse_state_t state;
|
||||
uint16_t data_len;
|
||||
uint16_t data_idx;
|
||||
uint8_t data[MMWAVE_BUF_SIZE];
|
||||
} ld2410_parser_t;
|
||||
|
||||
static ld2410_parser_t s_ld;
|
||||
|
||||
static void ld2410_process_frame(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
s_state.frame_count++;
|
||||
s_state.last_update_us = esp_timer_get_time();
|
||||
|
||||
if (len < 2) return;
|
||||
|
||||
uint8_t frame_type = data[0]; /* 0x01 = engineering, 0x02 = target */
|
||||
|
||||
if (frame_type == 0x02 && len >= 8) {
|
||||
/* Target report frame:
|
||||
* [0] frame_type=0x02
|
||||
* [1] target_state (0=none, 1=moving, 2=static, 3=both)
|
||||
* [2..3] moving_distance (cm, LE u16)
|
||||
* [4] moving_energy (0-100)
|
||||
* [5..6] static_distance (cm, LE u16)
|
||||
* [7] static_energy (0-100)
|
||||
*/
|
||||
uint8_t target_state = data[1];
|
||||
uint16_t moving_dist = data[2] | ((uint16_t)data[3] << 8);
|
||||
uint16_t static_dist = data[5] | ((uint16_t)data[6] << 8);
|
||||
|
||||
s_state.person_present = (target_state != 0);
|
||||
s_state.target_count = (target_state != 0) ? 1 : 0;
|
||||
|
||||
/* Use closest target distance. */
|
||||
if (target_state == 1 || target_state == 3) {
|
||||
s_state.distance_cm = (float)moving_dist;
|
||||
} else if (target_state == 2) {
|
||||
s_state.distance_cm = (float)static_dist;
|
||||
} else {
|
||||
s_state.distance_cm = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void ld2410_feed_byte(uint8_t b)
|
||||
{
|
||||
switch (s_ld.state) {
|
||||
case LD_WAIT_F4: s_ld.state = (b == 0xF4) ? LD_WAIT_F3 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F3: s_ld.state = (b == 0xF3) ? LD_WAIT_F2 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F2: s_ld.state = (b == 0xF2) ? LD_WAIT_F1 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F1: s_ld.state = (b == 0xF1) ? LD_READ_LEN_L : LD_WAIT_F4; break;
|
||||
case LD_READ_LEN_L:
|
||||
s_ld.data_len = b;
|
||||
s_ld.state = LD_READ_LEN_H;
|
||||
break;
|
||||
case LD_READ_LEN_H:
|
||||
s_ld.data_len |= ((uint16_t)b << 8);
|
||||
s_ld.data_idx = 0;
|
||||
if (s_ld.data_len == 0 || s_ld.data_len > MMWAVE_BUF_SIZE) {
|
||||
s_ld.state = LD_WAIT_F4;
|
||||
} else {
|
||||
s_ld.state = LD_READ_DATA;
|
||||
}
|
||||
break;
|
||||
case LD_READ_DATA:
|
||||
s_ld.data[s_ld.data_idx++] = b;
|
||||
if (s_ld.data_idx >= s_ld.data_len) s_ld.state = LD_WAIT_F8;
|
||||
break;
|
||||
case LD_WAIT_F8: s_ld.state = (b == 0xF8) ? LD_WAIT_F7 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F7: s_ld.state = (b == 0xF7) ? LD_WAIT_F6 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F6: s_ld.state = (b == 0xF6) ? LD_WAIT_F5 : LD_WAIT_F4; break;
|
||||
case LD_WAIT_F5:
|
||||
if (b == 0xF5) {
|
||||
ld2410_process_frame(s_ld.data, s_ld.data_len);
|
||||
}
|
||||
s_ld.state = LD_WAIT_F4;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ======================================================================
|
||||
* Mock mmWave Generator (for QEMU testing)
|
||||
* ====================================================================== */
|
||||
|
||||
#ifdef CONFIG_CSI_MOCK_ENABLED
|
||||
|
||||
static void mock_mmwave_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
ESP_LOGI(TAG, "Mock mmWave generator started (simulating MR60BHA2)");
|
||||
|
||||
s_state.type = MMWAVE_TYPE_MOCK;
|
||||
s_state.detected = true;
|
||||
s_state.capabilities = MMWAVE_CAP_HEART_RATE | MMWAVE_CAP_BREATHING
|
||||
| MMWAVE_CAP_PRESENCE | MMWAVE_CAP_DISTANCE;
|
||||
|
||||
float hr_base = 72.0f;
|
||||
float br_base = 16.0f;
|
||||
uint32_t tick = 0;
|
||||
|
||||
while (s_running) {
|
||||
tick++;
|
||||
|
||||
/* Simulate realistic vital sign variation. */
|
||||
float hr_noise = 2.0f * sinf((float)tick * 0.1f) + 0.5f * sinf((float)tick * 0.37f);
|
||||
float br_noise = 1.0f * sinf((float)tick * 0.07f) + 0.3f * sinf((float)tick * 0.23f);
|
||||
|
||||
s_state.heart_rate_bpm = hr_base + hr_noise;
|
||||
s_state.breathing_rate = br_base + br_noise;
|
||||
s_state.person_present = true;
|
||||
s_state.distance_cm = 150.0f + 20.0f * sinf((float)tick * 0.05f);
|
||||
s_state.target_count = 1;
|
||||
s_state.frame_count++;
|
||||
s_state.last_update_us = esp_timer_get_time();
|
||||
|
||||
/* Simulate person leaving at tick 200-250 (for scenario testing). */
|
||||
if (tick >= 200 && tick <= 250) {
|
||||
s_state.person_present = false;
|
||||
s_state.heart_rate_bpm = 0.0f;
|
||||
s_state.breathing_rate = 0.0f;
|
||||
s_state.distance_cm = 0.0f;
|
||||
s_state.target_count = 0;
|
||||
}
|
||||
|
||||
/* ~1 Hz update rate (matches real MR60BHA2). */
|
||||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||||
}
|
||||
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
#endif /* CONFIG_CSI_MOCK_ENABLED */
|
||||
|
||||
/* ======================================================================
|
||||
* UART Auto-Detection and Task
|
||||
* ====================================================================== */
|
||||
|
||||
#ifndef CONFIG_CSI_MOCK_ENABLED
|
||||
|
||||
/**
|
||||
* Probe UART for known sensor headers.
|
||||
* Reads bytes for MMWAVE_PROBE_TIMEOUT_MS and checks for MR60BHA2 or LD2410 headers.
|
||||
*/
|
||||
static mmwave_type_t probe_sensor(void)
|
||||
{
|
||||
uint8_t buf[128];
|
||||
int mr60_header_seen = 0;
|
||||
int ld2410_header_seen = 0;
|
||||
|
||||
int64_t deadline = esp_timer_get_time() + (int64_t)MMWAVE_PROBE_TIMEOUT_MS * 1000;
|
||||
|
||||
while (esp_timer_get_time() < deadline) {
|
||||
int len = uart_read_bytes(MMWAVE_UART_NUM, buf, sizeof(buf), pdMS_TO_TICKS(100));
|
||||
if (len <= 0) continue;
|
||||
|
||||
for (int i = 0; i < len - 1; i++) {
|
||||
/* MR60BHA2: 0x53 0x59 */
|
||||
if (buf[i] == 0x53 && buf[i + 1] == 0x59) {
|
||||
mr60_header_seen++;
|
||||
}
|
||||
/* LD2410: 0xF4 0xF3 */
|
||||
if (i + 3 < len && buf[i] == 0xF4 && buf[i+1] == 0xF3
|
||||
&& buf[i+2] == 0xF2 && buf[i+3] == 0xF1) {
|
||||
ld2410_header_seen++;
|
||||
}
|
||||
}
|
||||
|
||||
/* If we've seen multiple headers, we're confident. */
|
||||
if (mr60_header_seen >= 2) return MMWAVE_TYPE_MR60BHA2;
|
||||
if (ld2410_header_seen >= 2) return MMWAVE_TYPE_LD2410;
|
||||
}
|
||||
|
||||
/* Return best guess if we saw at least one header. */
|
||||
if (mr60_header_seen > 0) return MMWAVE_TYPE_MR60BHA2;
|
||||
if (ld2410_header_seen > 0) return MMWAVE_TYPE_LD2410;
|
||||
|
||||
return MMWAVE_TYPE_NONE;
|
||||
}
|
||||
|
||||
static void mmwave_uart_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
ESP_LOGI(TAG, "mmWave UART task started (type=%s)",
|
||||
mmwave_type_name(s_state.type));
|
||||
|
||||
uint8_t buf[128];
|
||||
|
||||
while (s_running) {
|
||||
int len = uart_read_bytes(MMWAVE_UART_NUM, buf, sizeof(buf), pdMS_TO_TICKS(100));
|
||||
if (len <= 0) {
|
||||
vTaskDelay(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int i = 0; i < len; i++) {
|
||||
if (s_state.type == MMWAVE_TYPE_MR60BHA2) {
|
||||
mr60_feed_byte(buf[i]);
|
||||
} else if (s_state.type == MMWAVE_TYPE_LD2410) {
|
||||
ld2410_feed_byte(buf[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/* Yield to prevent watchdog starvation. */
|
||||
vTaskDelay(1);
|
||||
}
|
||||
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
#endif /* !CONFIG_CSI_MOCK_ENABLED */
|
||||
|
||||
/* ======================================================================
|
||||
* Public API
|
||||
* ====================================================================== */
|
||||
|
||||
const char *mmwave_type_name(mmwave_type_t type)
|
||||
{
|
||||
switch (type) {
|
||||
case MMWAVE_TYPE_MR60BHA2: return "MR60BHA2";
|
||||
case MMWAVE_TYPE_LD2410: return "LD2410";
|
||||
case MMWAVE_TYPE_MOCK: return "Mock";
|
||||
case MMWAVE_TYPE_NONE:
|
||||
default: return "None";
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t mmwave_sensor_init(int uart_tx_pin, int uart_rx_pin)
|
||||
{
|
||||
memset(&s_state, 0, sizeof(s_state));
|
||||
memset(&s_mr60, 0, sizeof(s_mr60));
|
||||
memset(&s_ld, 0, sizeof(s_ld));
|
||||
s_running = true;
|
||||
|
||||
#ifdef CONFIG_CSI_MOCK_ENABLED
|
||||
/* Under QEMU: use mock generator instead of real UART. */
|
||||
ESP_LOGI(TAG, "Mock mode: starting synthetic mmWave generator");
|
||||
|
||||
BaseType_t ret = xTaskCreatePinnedToCore(
|
||||
mock_mmwave_task, "mmwave_mock", MMWAVE_TASK_STACK,
|
||||
NULL, MMWAVE_TASK_PRIORITY, NULL, 0);
|
||||
|
||||
if (ret != pdPASS) {
|
||||
ESP_LOGE(TAG, "Failed to create mock mmWave task");
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
return ESP_OK;
|
||||
|
||||
#else
|
||||
/* Real hardware: configure UART and probe for sensor. */
|
||||
if (uart_tx_pin < 0) uart_tx_pin = 17; /* Default GPIO17 */
|
||||
if (uart_rx_pin < 0) uart_rx_pin = 18; /* Default GPIO18 */
|
||||
|
||||
uart_config_t uart_config = {
|
||||
.baud_rate = MMWAVE_UART_BAUD,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
.source_clk = UART_SCLK_DEFAULT,
|
||||
};
|
||||
|
||||
esp_err_t err = uart_driver_install(MMWAVE_UART_NUM, MMWAVE_BUF_SIZE * 2, 0, 0, NULL, 0);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "UART driver install failed: %s", esp_err_to_name(err));
|
||||
return err;
|
||||
}
|
||||
|
||||
uart_param_config(MMWAVE_UART_NUM, &uart_config);
|
||||
uart_set_pin(MMWAVE_UART_NUM, uart_tx_pin, uart_rx_pin,
|
||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||||
|
||||
ESP_LOGI(TAG, "Probing UART%d (TX=%d, RX=%d) for mmWave sensor...",
|
||||
MMWAVE_UART_NUM, uart_tx_pin, uart_rx_pin);
|
||||
|
||||
mmwave_type_t detected = probe_sensor();
|
||||
|
||||
if (detected == MMWAVE_TYPE_NONE) {
|
||||
ESP_LOGI(TAG, "No mmWave sensor detected on UART%d", MMWAVE_UART_NUM);
|
||||
uart_driver_delete(MMWAVE_UART_NUM);
|
||||
return ESP_ERR_NOT_FOUND;
|
||||
}
|
||||
|
||||
s_state.type = detected;
|
||||
s_state.detected = true;
|
||||
|
||||
/* Register capabilities based on sensor type. */
|
||||
switch (detected) {
|
||||
case MMWAVE_TYPE_MR60BHA2:
|
||||
s_state.capabilities = MMWAVE_CAP_HEART_RATE | MMWAVE_CAP_BREATHING
|
||||
| MMWAVE_CAP_PRESENCE | MMWAVE_CAP_DISTANCE;
|
||||
break;
|
||||
case MMWAVE_TYPE_LD2410:
|
||||
s_state.capabilities = MMWAVE_CAP_PRESENCE | MMWAVE_CAP_DISTANCE;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "Detected %s (caps=0x%04x)", mmwave_type_name(detected),
|
||||
s_state.capabilities);
|
||||
|
||||
/* Start UART reader task. */
|
||||
BaseType_t ret = xTaskCreatePinnedToCore(
|
||||
mmwave_uart_task, "mmwave_uart", MMWAVE_TASK_STACK,
|
||||
NULL, MMWAVE_TASK_PRIORITY, NULL, 0);
|
||||
|
||||
if (ret != pdPASS) {
|
||||
ESP_LOGE(TAG, "Failed to create mmWave UART task");
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
return ESP_OK;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool mmwave_sensor_get_state(mmwave_state_t *state)
|
||||
{
|
||||
if (!s_state.detected || state == NULL) return false;
|
||||
memcpy(state, &s_state, sizeof(mmwave_state_t));
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
/**
|
||||
* @file mmwave_sensor.h
|
||||
* @brief ADR-063: 60 GHz mmWave sensor auto-detection and UART driver.
|
||||
*
|
||||
* Supports:
|
||||
* - Seeed MR60BHA2 (60 GHz, heart rate + breathing + presence)
|
||||
* - HLK-LD2410 (24 GHz, presence + distance)
|
||||
*
|
||||
* Auto-detects sensor type at boot by probing UART for known frame headers.
|
||||
* Runs a background task that parses incoming frames and updates shared state.
|
||||
*/
|
||||
|
||||
#ifndef MMWAVE_SENSOR_H
|
||||
#define MMWAVE_SENSOR_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "esp_err.h"
|
||||
|
||||
/* ---- Sensor type enumeration ---- */
|
||||
typedef enum {
|
||||
MMWAVE_TYPE_NONE = 0, /**< No sensor detected. */
|
||||
MMWAVE_TYPE_MR60BHA2 = 1, /**< Seeed MR60BHA2 (60 GHz, HR + BR). */
|
||||
MMWAVE_TYPE_LD2410 = 2, /**< HLK-LD2410 (24 GHz, presence + range). */
|
||||
MMWAVE_TYPE_MOCK = 99, /**< Mock sensor for QEMU testing. */
|
||||
} mmwave_type_t;
|
||||
|
||||
/* ---- Capability flags ---- */
|
||||
#define MMWAVE_CAP_HEART_RATE (1 << 0)
|
||||
#define MMWAVE_CAP_BREATHING (1 << 1)
|
||||
#define MMWAVE_CAP_PRESENCE (1 << 2)
|
||||
#define MMWAVE_CAP_DISTANCE (1 << 3)
|
||||
#define MMWAVE_CAP_FALL (1 << 4)
|
||||
#define MMWAVE_CAP_MULTI_TARGET (1 << 5)
|
||||
|
||||
/* ---- Shared mmWave state (updated by background task) ---- */
|
||||
typedef struct {
|
||||
/* Detection */
|
||||
mmwave_type_t type; /**< Detected sensor type. */
|
||||
uint16_t capabilities; /**< Bitmask of MMWAVE_CAP_* flags. */
|
||||
bool detected; /**< True if sensor responded on UART. */
|
||||
|
||||
/* Vital signs (MR60BHA2) */
|
||||
float heart_rate_bpm; /**< Heart rate in BPM (0 if unavailable). */
|
||||
float breathing_rate; /**< Breathing rate in breaths/min. */
|
||||
|
||||
/* Presence and range (LD2410 / MR60BHA2) */
|
||||
bool person_present; /**< True if person detected. */
|
||||
float distance_cm; /**< Distance to nearest target in cm. */
|
||||
uint8_t target_count; /**< Number of detected targets. */
|
||||
|
||||
/* Quality metrics */
|
||||
uint32_t frame_count; /**< Total parsed frames since boot. */
|
||||
uint32_t error_count; /**< Parse errors / CRC failures. */
|
||||
int64_t last_update_us; /**< Timestamp of last valid frame. */
|
||||
} mmwave_state_t;
|
||||
|
||||
/**
|
||||
* Initialize the mmWave sensor subsystem.
|
||||
*
|
||||
* Probes the configured UART for known sensor types. If a sensor is
|
||||
* detected, starts a background FreeRTOS task to parse incoming frames.
|
||||
*
|
||||
* @param uart_tx_pin GPIO pin for UART TX (to sensor RX). Use -1 for default.
|
||||
* @param uart_rx_pin GPIO pin for UART RX (from sensor TX). Use -1 for default.
|
||||
* @return ESP_OK if sensor detected, ESP_ERR_NOT_FOUND if no sensor.
|
||||
*/
|
||||
esp_err_t mmwave_sensor_init(int uart_tx_pin, int uart_rx_pin);
|
||||
|
||||
/**
|
||||
* Get a snapshot of the current mmWave state (thread-safe copy).
|
||||
*
|
||||
* @param state Output state struct.
|
||||
* @return true if valid data is available (sensor detected and running).
|
||||
*/
|
||||
bool mmwave_sensor_get_state(mmwave_state_t *state);
|
||||
|
||||
/**
|
||||
* Get the detected sensor type name as a string.
|
||||
*/
|
||||
const char *mmwave_type_name(mmwave_type_t type);
|
||||
|
||||
#endif /* MMWAVE_SENSOR_H */
|
||||
|
|
@ -63,3 +63,13 @@ esp_err_t wasm_runtime_unload(uint8_t id) { (void)id; return ESP_OK; }
|
|||
void wasm_runtime_on_timer(void) {}
|
||||
void wasm_runtime_get_info(wasm_module_info_t *info, uint8_t *count) { (void)info; if(count) *count = 0; }
|
||||
esp_err_t wasm_runtime_set_manifest(uint8_t id, const char *n, uint32_t c, uint32_t m) { (void)id; (void)n; (void)c; (void)m; return ESP_OK; }
|
||||
|
||||
/* ---- mmwave_sensor stubs (ADR-063) ---- */
|
||||
|
||||
#include "mmwave_sensor.h"
|
||||
|
||||
static mmwave_state_t s_stub_mmwave = {0};
|
||||
|
||||
esp_err_t mmwave_sensor_init(int tx, int rx) { (void)tx; (void)rx; return ESP_ERR_NOT_FOUND; }
|
||||
bool mmwave_sensor_get_state(mmwave_state_t *s) { if (s) *s = s_stub_mmwave; return false; }
|
||||
const char *mmwave_type_name(mmwave_type_t t) { (void)t; return "None"; }
|
||||
|
|
|
|||
Loading…
Reference in New Issue