berkus
/
wifi-densepose
mirror of https://github.com/ruvnet/wifi-densepose.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
wifi-densepose/firmware/esp32-csi-node/main/mmwave_sensor.c

534 lines
16 KiB
C
Raw Blame History

/**
* @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;
}
Reference in New Issue View Git Blame Copy Permalink