/** * @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 #include #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; }