除了显示驱动，其余全部测试通过
This commit is contained in:
@@ -0,0 +1,449 @@
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user