#include "system/includes.h" #include "system/timer.h" #include "app_log.h" #include "radar_command_service.h" #include "r60abd1.h" #define RADAR_COMMAND_FRAME_LEN 10 #define RADAR_COMMAND_STARTUP_INTERVAL_MS 50 #define RADAR_COMMAND_PERIODIC_INTERVAL_MS 2000 #define RADAR_COMMAND_SKIP_INTERVAL_MS 100 #define RADAR_COMMAND_RETRY_INTERVAL_MS 100 #define RADAR_COMMAND_TASK_PRIO 20 #define RADAR_COMMAND_TASK_STACK 1024 enum radar_command_phase { RADAR_COMMAND_PHASE_STARTUP = 0, RADAR_COMMAND_PHASE_PERIODIC }; /* * ESP32参考工程启动阶段： * * 1. queryPresenceCmd原始10字节 * 2. 12条startupCommands * * 每条之间50ms。 */ static const u8 radar_startup_commands[][RADAR_COMMAND_FRAME_LEN] = { {0x53, 0x59, 0x80, 0x81, 0x00, 0x01, 0x00, 0xAE, 0x54, 0x43}, {0x53, 0x59, 0x80, 0x00, 0x00, 0x01, 0x01, 0x2E, 0x54, 0x43}, {0x53, 0x59, 0x81, 0x00, 0x00, 0x01, 0x01, 0x2F, 0x54, 0x43}, {0x53, 0x59, 0x85, 0x00, 0x00, 0x01, 0x01, 0x33, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x00, 0x00, 0x01, 0x01, 0x32, 0x54, 0x43}, {0x53, 0x59, 0x81, 0x0C, 0x00, 0x01, 0x01, 0x3B, 0x54, 0x43}, {0x53, 0x59, 0x85, 0x0A, 0x00, 0x01, 0x01, 0x3D, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x13, 0x00, 0x01, 0x01, 0x45, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x14, 0x00, 0x01, 0x01, 0x46, 0x54, 0x43}, {0x53, 0x59, 0x80, 0x80, 0x00, 0x01, 0x0F, 0xBC, 0x54, 0x43}, {0x53, 0x59, 0x81, 0x80, 0x00, 0x01, 0x0F, 0xBD, 0x54, 0x43}, {0x53, 0x59, 0x85, 0x80, 0x00, 0x01, 0x0F, 0xC1, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x80, 0x00, 0x01, 0x0F, 0xC0, 0x54, 0x43}, }; /* * 周期查询命令： * * 84系列保持ESP32参考任务的查询顺序； * 心率/呼吸数值查询按R60ABD1 V3.4手册使用85/82、81/82。 * 每条value固定为0x0F，正常每2秒发送一条。 */ static const u8 radar_periodic_commands[][RADAR_COMMAND_FRAME_LEN] = { {0x53, 0x59, 0x84, 0x81, 0x00, 0x01, 0x0F, 0xC1, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x8D, 0x00, 0x01, 0x0F, 0xCD, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x8F, 0x00, 0x01, 0x0F, 0xCF, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x8E, 0x00, 0x01, 0x0F, 0xCE, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x91, 0x00, 0x01, 0x0F, 0xD1, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x92, 0x00, 0x01, 0x0F, 0xD2, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x83, 0x00, 0x01, 0x0F, 0xC3, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x84, 0x00, 0x01, 0x0F, 0xC4, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x85, 0x00, 0x01, 0x0F, 0xC5, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x86, 0x00, 0x01, 0x0F, 0xC6, 0x54, 0x43}, {0x53, 0x59, 0x84, 0x90, 0x00, 0x01, 0x0F, 0xD0, 0x54, 0x43}, {0x53, 0x59, 0x85, 0x82, 0x00, 0x01, 0x0F, 0xC3, 0x54, 0x43}, {0x53, 0x59, 0x81, 0x82, 0x00, 0x01, 0x0F, 0xBF, 0x54, 0x43}, }; static enum radar_command_phase radar_command_phase; static u8 radar_command_sleep_query_enabled; static u16 radar_command_startup_cmd_index; static u16 radar_command_periodic_query_index; static u32 radar_command_next_ms; static int radar_command_task_pid; /* * ESP32中可关闭的睡眠查询： * periodic_query_index 1 -> 84/8D * periodic_query_index 10 -> 84/90 */ static int radar_command_is_sleep_query(u16 query_index) { return (query_index == 1) || (query_index == 10); } static int radar_command_time_reached( u32 now, u32 deadline ) { /* * 兼容u32毫秒计数回绕。 */ return ((s32)(now - deadline) >= 0); } static int radar_command_send_frame( const u8 *frame ) { int ret; ret = radar_r60abd1_send_command( frame, RADAR_COMMAND_FRAME_LEN ); if (ret < 0) { return ret; } /* * 当前UART写接口成功时应返回实际写入长度。 */ if (ret != RADAR_COMMAND_FRAME_LEN) { return -2; } return 0; } /* * 单次非阻塞调度。 * * 只由本模块内部任务调用。 */ static int radar_command_process_once(void) { int ret; u32 now; const u8 *frame; now = timer_get_ms(); if (!radar_command_time_reached( now, radar_command_next_ms )) { return 0; } /* * 第一阶段： * 13条启动命令，50ms间隔。 */ if (radar_command_phase == RADAR_COMMAND_PHASE_STARTUP) { frame = radar_startup_commands[ radar_command_startup_cmd_index ]; ret = radar_command_send_frame(frame); if (ret != 0) { radar_command_next_ms = now + RADAR_COMMAND_RETRY_INTERVAL_MS; APP_LOG( "[RADAR_CMD] startup send failed " "startup_cmd_index=%u ctrl=%02X cmd=%02X ret=%d\n", radar_command_startup_cmd_index, frame[2], frame[3], ret ); return ret; } APP_LOG( "[RADAR_CMD] startup sent " "startup_cmd_index=%u ctrl=%02X cmd=%02X value=%02X\n", radar_command_startup_cmd_index, frame[2], frame[3], frame[6] ); radar_command_startup_cmd_index++; if (radar_command_startup_cmd_index >= (sizeof(radar_startup_commands) / sizeof(radar_startup_commands[0]))) { radar_command_phase = RADAR_COMMAND_PHASE_PERIODIC; radar_command_periodic_query_index = 0; radar_command_next_ms = now + RADAR_COMMAND_PERIODIC_INTERVAL_MS; APP_LOG( "[RADAR_CMD] startup complete\n" ); } else { radar_command_next_ms = now + RADAR_COMMAND_STARTUP_INTERVAL_MS; } return 1; } /* * 第二阶段： * 周期查询。 */ if ((!radar_command_sleep_query_enabled) && radar_command_is_sleep_query( radar_command_periodic_query_index )) { APP_LOG( "[RADAR_CMD] periodic skip " "periodic_query_index=%u ctrl=%02X cmd=%02X\n", radar_command_periodic_query_index, radar_periodic_commands[ radar_command_periodic_query_index ][2], radar_periodic_commands[ radar_command_periodic_query_index ][3] ); radar_command_periodic_query_index = (u16)( (radar_command_periodic_query_index + 1) % ( sizeof(radar_periodic_commands) / sizeof(radar_periodic_commands[0]) ) ); radar_command_next_ms = now + RADAR_COMMAND_SKIP_INTERVAL_MS; return 0; } frame = radar_periodic_commands[ radar_command_periodic_query_index ]; ret = radar_command_send_frame(frame); if (ret != 0) { radar_command_next_ms = now + RADAR_COMMAND_RETRY_INTERVAL_MS; APP_LOG( "[RADAR_CMD] periodic send failed " "periodic_query_index=%u ctrl=%02X cmd=%02X ret=%d\n", radar_command_periodic_query_index, frame[2], frame[3], ret ); return ret; } APP_LOG( "[RADAR_CMD] periodic sent " "periodic_query_index=%u ctrl=%02X cmd=%02X value=%02X\n", radar_command_periodic_query_index, frame[2], frame[3], frame[6] ); radar_command_periodic_query_index = (u16)( (radar_command_periodic_query_index + 1) % ( sizeof(radar_periodic_commands) / sizeof(radar_periodic_commands[0]) ) ); radar_command_next_ms = now + RADAR_COMMAND_PERIODIC_INTERVAL_MS; return 1; } static void radar_command_task(void *priv) { (void)priv; while (1) { /* * process_once自身按timer_get_ms()判断是否到期。 * 这里仅短暂让出CPU，不依赖app_main的UART阻塞周期。 */ radar_command_process_once(); os_time_dly(1); } } int radar_command_service_init(void) { int ret; if (radar_command_task_pid) { return 0; } radar_command_phase = RADAR_COMMAND_PHASE_STARTUP; radar_command_sleep_query_enabled = 1; radar_command_startup_cmd_index = 0; radar_command_periodic_query_index = 0; /* * 任务启动后允许立即发送首条queryPresenceCmd。 */ radar_command_next_ms = timer_get_ms(); ret = thread_fork( "radar_cmd", RADAR_COMMAND_TASK_PRIO, RADAR_COMMAND_TASK_STACK, 0, &radar_command_task_pid, radar_command_task, NULL ); if (ret != OS_NO_ERR) { radar_command_task_pid = 0; APP_LOG( "[RADAR_CMD] task start failed ret=%d\n", ret ); return -1; } APP_LOG( "[RADAR_CMD] init success " "startup=%u periodic=%u\n", (u32)( sizeof(radar_startup_commands) / sizeof(radar_startup_commands[0]) ), (u32)( sizeof(radar_periodic_commands) / sizeof(radar_periodic_commands[0]) ) ); return 0; } void radar_command_service_set_sleep_query_enabled( u8 enabled ) { radar_command_sleep_query_enabled = enabled ? 1 : 0; APP_LOG( "[RADAR_CMD] sleep query %s\n", radar_command_sleep_query_enabled ? "enabled" : "disabled" ); } u8 radar_command_service_get_sleep_query_enabled(void) { return radar_command_sleep_query_enabled; }