449 lines
9.8 KiB
C
449 lines
9.8 KiB
C
#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;
|
|
} |