726 lines
13 KiB
C
726 lines
13 KiB
C
#include "system/includes.h"
|
||
#include "system/timer.h"
|
||
#include "app_log.h"
|
||
#include "r60abd1.h"
|
||
#include "radar_uart.h"
|
||
#include <string.h>
|
||
|
||
|
||
/*
|
||
* R60ABD1协议格式：
|
||
*
|
||
* [0] 0x53
|
||
* [1] 0x59
|
||
* [2] CTRL
|
||
* [3] CMD
|
||
* [4] DATA_LEN_H
|
||
* [5] DATA_LEN_L
|
||
* [6..] DATA
|
||
* [-3] CHECKSUM
|
||
* [-2] 0x54
|
||
* [-1] 0x43
|
||
*
|
||
* full_len = 9 + data_len
|
||
*/
|
||
|
||
#define R60_FRAME_HEADER1 0x53
|
||
#define R60_FRAME_HEADER2 0x59
|
||
#define R60_FRAME_TAIL1 0x54
|
||
#define R60_FRAME_TAIL2 0x43
|
||
|
||
#define R60_CTRL_PRESENCE 0x80
|
||
#define R60_CTRL_BREATH 0x81
|
||
#define R60_CTRL_SLEEP 0x84
|
||
#define R60_CTRL_HEART 0x85
|
||
|
||
#define R60_PARSE_BUF_SIZE 128
|
||
|
||
|
||
#ifndef R60ABD1_DEBUG_LOG
|
||
#define R60ABD1_DEBUG_LOG 1
|
||
#endif
|
||
|
||
|
||
#if R60ABD1_DEBUG_LOG
|
||
#define R60_LOG(...) APP_LOG(__VA_ARGS__)
|
||
#else
|
||
#define R60_LOG(...)
|
||
#endif
|
||
|
||
|
||
static u8 r60_parse_buf[R60_PARSE_BUF_SIZE];
|
||
static u16 r60_parse_len;
|
||
|
||
static u32 r60_rx_bytes;
|
||
static u32 r60_valid_frame_count;
|
||
static u32 r60_data_update_count;
|
||
static u32 r60_checksum_error_count;
|
||
|
||
static struct radar_r60abd1_data radar_data;
|
||
|
||
static OS_MUTEX radar_data_mutex;
|
||
static u8 radar_data_mutex_ready;
|
||
|
||
|
||
/*
|
||
* R60坐标格式：
|
||
*
|
||
* bit15 = 符号位
|
||
* bit14..0 = 数值绝对值
|
||
*/
|
||
static s16 r60_parse_signed_coordinate(u16 raw_value)
|
||
{
|
||
s16 value = (s16)(raw_value & 0x7fff);
|
||
|
||
if (raw_value & 0x8000) {
|
||
value = -value;
|
||
}
|
||
|
||
return value;
|
||
}
|
||
|
||
|
||
/*
|
||
* 协议层初始化。
|
||
*
|
||
* 必须在：
|
||
* radar_r60abd1_input_bytes()
|
||
* radar_r60abd1_get_data()
|
||
*
|
||
* 之前调用。
|
||
*/
|
||
int radar_r60abd1_init(void)
|
||
{
|
||
memset(r60_parse_buf, 0, sizeof(r60_parse_buf));
|
||
memset(&radar_data, 0, sizeof(radar_data));
|
||
|
||
r60_parse_len = 0;
|
||
r60_rx_bytes = 0;
|
||
r60_valid_frame_count = 0;
|
||
r60_data_update_count = 0;
|
||
r60_checksum_error_count = 0;
|
||
|
||
if (!radar_data_mutex_ready) {
|
||
os_mutex_create(&radar_data_mutex);
|
||
radar_data_mutex_ready = 1;
|
||
}
|
||
|
||
R60_LOG("[R60] init success\n");
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/*
|
||
* 完整帧合法性校验。
|
||
*/
|
||
static int r60_validate_frame(const u8 *frame, u16 frame_len)
|
||
{
|
||
u8 checksum = 0;
|
||
u16 data_len;
|
||
u16 i;
|
||
|
||
if (!frame || frame_len < 9) {
|
||
return -1;
|
||
}
|
||
|
||
if (frame[0] != R60_FRAME_HEADER1 ||
|
||
frame[1] != R60_FRAME_HEADER2) {
|
||
return -2;
|
||
}
|
||
|
||
data_len =
|
||
((u16)frame[4] << 8) |
|
||
frame[5];
|
||
|
||
if ((u16)(9 + data_len) != frame_len) {
|
||
return -3;
|
||
}
|
||
|
||
if (frame[frame_len - 2] != R60_FRAME_TAIL1 ||
|
||
frame[frame_len - 1] != R60_FRAME_TAIL2) {
|
||
return -4;
|
||
}
|
||
|
||
/*
|
||
* checksum:
|
||
*
|
||
* frame[0] 到 DATA最后一个字节之和。
|
||
* 不包含checksum本身和帧尾。
|
||
*/
|
||
for (i = 0; i < frame_len - 3; i++) {
|
||
checksum += frame[i];
|
||
}
|
||
|
||
if (checksum != frame[frame_len - 3]) {
|
||
|
||
if (radar_data_mutex_ready) {
|
||
|
||
os_mutex_pend(&radar_data_mutex, 0);
|
||
|
||
r60_checksum_error_count++;
|
||
radar_data.checksum_errors =
|
||
r60_checksum_error_count;
|
||
|
||
os_mutex_post(&radar_data_mutex);
|
||
|
||
} else {
|
||
|
||
r60_checksum_error_count++;
|
||
}
|
||
|
||
R60_LOG("[R60] checksum error: calc=%02X recv=%02X\n",
|
||
checksum,
|
||
frame[frame_len - 3]);
|
||
|
||
return -5;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/*
|
||
* 业务解析。
|
||
*
|
||
* 只负责：
|
||
* 1. CTRL/CMD解释
|
||
* 2. 更新RAM中的最新状态
|
||
*
|
||
* 不做：
|
||
* Flash
|
||
* 网络上传
|
||
*/
|
||
static int r60_parse_frame(const u8 *frame, u16 frame_len)
|
||
{
|
||
u8 ctrl;
|
||
u8 cmd;
|
||
u16 data_len;
|
||
int updated = 0;
|
||
|
||
if (!frame || frame_len < 9) {
|
||
return 0;
|
||
}
|
||
|
||
if (!radar_data_mutex_ready) {
|
||
return 0;
|
||
}
|
||
|
||
ctrl = frame[2];
|
||
cmd = frame[3];
|
||
|
||
data_len =
|
||
((u16)frame[4] << 8) |
|
||
frame[5];
|
||
|
||
|
||
os_mutex_pend(&radar_data_mutex, 0);
|
||
|
||
|
||
switch (ctrl) {
|
||
|
||
/*
|
||
* 人体存在 / 运动 / 距离 / 坐标
|
||
*/
|
||
case R60_CTRL_PRESENCE:
|
||
|
||
switch (cmd) {
|
||
|
||
case 0x01:
|
||
case 0x81:
|
||
|
||
if (data_len >= 1) {
|
||
radar_data.presence = frame[6];
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] presence=%u\n",
|
||
radar_data.presence);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x02:
|
||
|
||
if (data_len >= 1) {
|
||
radar_data.motion = frame[6];
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] motion=%u\n",
|
||
radar_data.motion);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x03:
|
||
|
||
if (data_len >= 1) {
|
||
radar_data.body_movement = frame[6];
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] body_movement=%u\n",
|
||
radar_data.body_movement);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x04:
|
||
|
||
if (data_len >= 2) {
|
||
|
||
radar_data.distance_cm =
|
||
((u16)frame[6] << 8) |
|
||
frame[7];
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] distance=%u cm\n",
|
||
radar_data.distance_cm);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x05:
|
||
|
||
if (data_len >= 6) {
|
||
|
||
radar_data.pos_x_mm =
|
||
r60_parse_signed_coordinate(
|
||
((u16)frame[6] << 8) |
|
||
frame[7]);
|
||
|
||
radar_data.pos_y_mm =
|
||
r60_parse_signed_coordinate(
|
||
((u16)frame[8] << 8) |
|
||
frame[9]);
|
||
|
||
radar_data.pos_z_mm =
|
||
r60_parse_signed_coordinate(
|
||
((u16)frame[10] << 8) |
|
||
frame[11]);
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] pos=(%d,%d,%d) mm\n",
|
||
radar_data.pos_x_mm,
|
||
radar_data.pos_y_mm,
|
||
radar_data.pos_z_mm);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
/*
|
||
* 呼吸
|
||
*/
|
||
case R60_CTRL_BREATH:
|
||
|
||
switch (cmd) {
|
||
|
||
case 0x02:
|
||
case 0x82:
|
||
|
||
if (data_len >= 1) {
|
||
|
||
radar_data.breath_rate_x10 =
|
||
(u16)frame[6] * 10;
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] breath=%u /min\n",
|
||
radar_data.breath_rate_x10 / 10);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x05:
|
||
|
||
if (data_len >= 1) {
|
||
radar_data.breath_waveform = frame[6];
|
||
updated = 1;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
/*
|
||
* 心率
|
||
*/
|
||
case R60_CTRL_HEART:
|
||
|
||
switch (cmd) {
|
||
|
||
case 0x02:
|
||
case 0x82:
|
||
|
||
if (data_len >= 1) {
|
||
|
||
radar_data.heart_rate_x10 =
|
||
(u16)frame[6] * 10;
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] heart=%u bpm\n",
|
||
radar_data.heart_rate_x10 / 10);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
case 0x05:
|
||
|
||
if (data_len >= 1) {
|
||
radar_data.heart_waveform = frame[6];
|
||
updated = 1;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
/*
|
||
* 睡眠
|
||
*/
|
||
case R60_CTRL_SLEEP:
|
||
|
||
switch (cmd) {
|
||
|
||
case 0x0c:
|
||
case 0x8d:
|
||
|
||
if (data_len >= 2) {
|
||
|
||
radar_data.presence = frame[6];
|
||
radar_data.sleep_state = frame[7];
|
||
updated = 1;
|
||
|
||
R60_LOG("[R60][PARSE] sleep presence=%u state=%u\n",
|
||
radar_data.presence,
|
||
radar_data.sleep_state);
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
|
||
/*
|
||
* 手册已确认、但当前公共状态结构尚未承载的合法帧，例如：
|
||
*
|
||
* ctrl=07 cmd=07
|
||
* ctrl=81 cmd=01
|
||
*
|
||
* 本阶段暂不映射，避免顺带扩展业务字段。
|
||
*/
|
||
default:
|
||
break;
|
||
}
|
||
|
||
|
||
/*
|
||
* 通信统计和业务新鲜度严格分开：
|
||
*
|
||
* frame_count:
|
||
* 所有checksum正确的合法帧都会增加。
|
||
*
|
||
* data_update_count / update_ms:
|
||
* 只有当前parser真正识别并更新业务字段时才推进。
|
||
*
|
||
* 因此07/07、01/01等未知合法帧不会把旧数据“续鲜”。
|
||
*/
|
||
radar_data.rx_bytes = r60_rx_bytes;
|
||
radar_data.frame_count = r60_valid_frame_count;
|
||
radar_data.checksum_errors =
|
||
r60_checksum_error_count;
|
||
|
||
if (updated) {
|
||
r60_data_update_count++;
|
||
radar_data.data_update_count =
|
||
r60_data_update_count;
|
||
radar_data.update_ms = timer_get_ms();
|
||
} else {
|
||
R60_LOG(
|
||
"[R60] ignored business frame ctrl=%02X cmd=%02X "
|
||
"frame=%u data=%u\n",
|
||
ctrl,
|
||
cmd,
|
||
r60_valid_frame_count,
|
||
r60_data_update_count
|
||
);
|
||
}
|
||
|
||
|
||
os_mutex_post(&radar_data_mutex);
|
||
|
||
return updated;
|
||
}
|
||
|
||
|
||
/*
|
||
* 收到一帧完整数据。
|
||
*/
|
||
static void r60_frame_received(const u8 *frame, u16 frame_len)
|
||
{
|
||
int ret;
|
||
u32 frame_count;
|
||
|
||
ret = r60_validate_frame(frame, frame_len);
|
||
|
||
if (ret != 0) {
|
||
|
||
R60_LOG("[R60] invalid frame, ret=%d\n",
|
||
ret);
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
if (!radar_data_mutex_ready) {
|
||
return;
|
||
}
|
||
|
||
|
||
os_mutex_pend(&radar_data_mutex, 0);
|
||
|
||
r60_valid_frame_count++;
|
||
|
||
radar_data.frame_count =
|
||
r60_valid_frame_count;
|
||
|
||
frame_count =
|
||
r60_valid_frame_count;
|
||
|
||
os_mutex_post(&radar_data_mutex);
|
||
|
||
|
||
R60_LOG("[R60] valid frame=%u len=%u ctrl=%02X cmd=%02X\n",
|
||
frame_count,
|
||
frame_len,
|
||
frame[2],
|
||
frame[3]);
|
||
|
||
|
||
r60_parse_frame(frame, frame_len);
|
||
}
|
||
|
||
|
||
/*
|
||
* 逐字节组帧。
|
||
*/
|
||
static void r60_feed_byte(u8 byte)
|
||
{
|
||
u16 data_len;
|
||
u16 full_len;
|
||
|
||
|
||
/*
|
||
* 找0x53。
|
||
*/
|
||
if (r60_parse_len == 0) {
|
||
|
||
if (byte != R60_FRAME_HEADER1) {
|
||
return;
|
||
}
|
||
|
||
r60_parse_buf[0] = byte;
|
||
r60_parse_len = 1;
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/*
|
||
* 已收到0x53，找0x59。
|
||
*/
|
||
if (r60_parse_len == 1) {
|
||
|
||
if (byte == R60_FRAME_HEADER2) {
|
||
|
||
r60_parse_buf[1] = byte;
|
||
r60_parse_len = 2;
|
||
|
||
return;
|
||
}
|
||
|
||
/*
|
||
* 53 53：
|
||
* 后一个53仍可能是新帧头。
|
||
*/
|
||
if (byte == R60_FRAME_HEADER1) {
|
||
return;
|
||
}
|
||
|
||
r60_parse_len = 0;
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/*
|
||
* parser buffer保护。
|
||
*/
|
||
if (r60_parse_len >= sizeof(r60_parse_buf)) {
|
||
|
||
R60_LOG("[R60] frame buffer overflow\n");
|
||
|
||
r60_parse_len = 0;
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
r60_parse_buf[r60_parse_len++] = byte;
|
||
|
||
|
||
/*
|
||
* 前6字节收齐后才能读取DATA_LEN。
|
||
*/
|
||
if (r60_parse_len < 6) {
|
||
return;
|
||
}
|
||
|
||
|
||
data_len =
|
||
((u16)r60_parse_buf[4] << 8) |
|
||
r60_parse_buf[5];
|
||
|
||
|
||
full_len =
|
||
(u16)(9 + data_len);
|
||
|
||
|
||
if (full_len > sizeof(r60_parse_buf)) {
|
||
|
||
R60_LOG("[R60] invalid frame length: data=%u full=%u\n",
|
||
data_len,
|
||
full_len);
|
||
|
||
r60_parse_len = 0;
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
if (r60_parse_len < full_len) {
|
||
return;
|
||
}
|
||
|
||
|
||
r60_frame_received(
|
||
r60_parse_buf,
|
||
full_len
|
||
);
|
||
|
||
|
||
r60_parse_len = 0;
|
||
}
|
||
|
||
|
||
/*
|
||
* UART层收到数据后调用。
|
||
*/
|
||
int radar_r60abd1_input_bytes(const u8 *data, u16 len)
|
||
{
|
||
u16 i;
|
||
|
||
if (!data || !len) {
|
||
return -1;
|
||
}
|
||
|
||
if (!radar_data_mutex_ready) {
|
||
return -2;
|
||
}
|
||
|
||
|
||
/*
|
||
* 统计UART输入字节数。
|
||
*/
|
||
os_mutex_pend(&radar_data_mutex, 0);
|
||
|
||
r60_rx_bytes += len;
|
||
radar_data.rx_bytes = r60_rx_bytes;
|
||
|
||
os_mutex_post(&radar_data_mutex);
|
||
|
||
|
||
/*
|
||
* 逐字节送入组帧器。
|
||
*/
|
||
for (i = 0; i < len; i++) {
|
||
r60_feed_byte(data[i]);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/*
|
||
* 获取当前最新雷达状态快照。
|
||
*
|
||
* 可供其他任务调用。
|
||
*/
|
||
int radar_r60abd1_get_data(struct radar_r60abd1_data *out)
|
||
{
|
||
if (!out) {
|
||
return -1;
|
||
}
|
||
|
||
if (!radar_data_mutex_ready) {
|
||
return -2;
|
||
}
|
||
|
||
|
||
os_mutex_pend(&radar_data_mutex, 0);
|
||
|
||
memcpy(out,
|
||
&radar_data,
|
||
sizeof(*out));
|
||
|
||
os_mutex_post(&radar_data_mutex);
|
||
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/*
|
||
* 向R60ABD1发送原始协议命令。
|
||
*
|
||
* 协议层只负责把完整命令交给UART层发送。
|
||
*/
|
||
int radar_r60abd1_send_command(const u8 *cmd, u16 len)
|
||
{
|
||
if (!cmd || !len) {
|
||
return -1;
|
||
}
|
||
|
||
return radar_uart_write(
|
||
cmd,
|
||
len
|
||
);
|
||
} |