553 lines
16 KiB
C++
553 lines
16 KiB
C++
/**
|
|
* @file radar_manager.cpp
|
|
* @brief R60ABD1 毫米波雷达管理模块实现
|
|
*
|
|
* 状态机逐字节解析 UART 帧, 提取生命体征数据
|
|
*/
|
|
|
|
#include "radar_manager.h"
|
|
|
|
// ============================================================
|
|
// 构造函数 & 析构函数
|
|
// ============================================================
|
|
|
|
RadarManager::RadarManager()
|
|
: _serial(nullptr)
|
|
, _state(STATE_IDLE)
|
|
, _dataIndex(0)
|
|
, _initialized(false)
|
|
, _callback(nullptr)
|
|
, _frameCount(0)
|
|
, _errorCount(0)
|
|
, _lastHeartBeat(0)
|
|
, _lastBreathBeat(0)
|
|
, _stablePresence(false)
|
|
, _presenceCnt(0)
|
|
, _lastRawPresence(false)
|
|
, _motionCnt(0)
|
|
, _lastRawMotion(false)
|
|
{
|
|
memset(&_currentFrame, 0, sizeof(_currentFrame));
|
|
memset(&_data, 0, sizeof(_data));
|
|
}
|
|
|
|
RadarManager::~RadarManager() {
|
|
}
|
|
|
|
// ============================================================
|
|
// 初始化
|
|
// ============================================================
|
|
|
|
bool RadarManager::begin() {
|
|
_serial = &RADAR_UART_PORT;
|
|
_serial->begin(RADAR_BAUD_RATE, SERIAL_8N1,
|
|
RADAR_RX_PIN, RADAR_TX_PIN);
|
|
|
|
// 可选: 初始化雷达 GPIO 作为辅助
|
|
pinMode(RADAR_GPIO1_PIN, INPUT);
|
|
pinMode(RADAR_GPIO2_PIN, INPUT);
|
|
|
|
_resetParser();
|
|
memset(&_data, 0, sizeof(_data));
|
|
_data.lastUpdate = millis();
|
|
_stablePresence = false;
|
|
_presenceCnt = 0;
|
|
_lastRawPresence = false;
|
|
|
|
// 不给雷达发任何命令，让它自己初始化并开始自动上报
|
|
// R60ABD1 上电后通常需要 15-30 秒自检，之后自动输出数据
|
|
Serial.println(F("[雷达] 雷达模块初始化完成, 等待自动上报..."));
|
|
Serial.println(F("[雷达] (R60ABD1 自检约15-30秒，心率和呼吸需检测到静止人体后输出)"));
|
|
return true;
|
|
}
|
|
|
|
// ============================================================
|
|
// 主循环 - 逐字节解析 UART 数据
|
|
// ============================================================
|
|
|
|
void RadarManager::loop() {
|
|
while (_serial->available() > 0) {
|
|
uint8_t byte = _serial->read();
|
|
_parseByte(byte);
|
|
}
|
|
|
|
// 首次收到数据时打印提示
|
|
static bool firstData = false;
|
|
if (!firstData && _frameCount > 0) {
|
|
firstData = true;
|
|
Serial.printf("[雷达] ✅ 开始接收数据 (首帧)\n");
|
|
}
|
|
|
|
// 心率/呼吸数据过期 (>10秒无更新 → 标记无效)
|
|
if (millis() - _lastHeartBeat > 10000) {
|
|
_data.vitalValid = false;
|
|
_data.heartRate = 0;
|
|
}
|
|
if (millis() - _lastBreathBeat > 10000) {
|
|
_data.vitalValid = false;
|
|
_data.breathRate = 0;
|
|
}
|
|
|
|
// 超时检测：如果 10 秒还没收到任何数据，打印一次
|
|
static uint32_t noDataWarn = 0;
|
|
if (_frameCount == 0 && millis() - _data.lastUpdate > 10000) {
|
|
if (millis() - noDataWarn > 10000) {
|
|
noDataWarn = millis();
|
|
Serial.println(F("[雷达] ⚠️ 10秒内未收到雷达数据, 请检查:"));
|
|
Serial.println(F("[雷达] 1.雷达供电 5V? 2.TX→GPIO17 RX→GPIO1? 3.交叉了吗?"));
|
|
}
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// 帧解析状态机
|
|
// ============================================================
|
|
|
|
void RadarManager::_parseByte(uint8_t byte) {
|
|
switch (_state) {
|
|
|
|
case STATE_IDLE:
|
|
// 等待帧头第一个字节 0x53
|
|
if (byte == FRAME_HEADER_1) {
|
|
_resetParser();
|
|
_currentFrame.data[0] = byte;
|
|
_state = STATE_GOT_HEADER1;
|
|
}
|
|
break;
|
|
|
|
case STATE_GOT_HEADER1:
|
|
// 等待帧头第二个字节 0x59
|
|
if (byte == FRAME_HEADER_2) {
|
|
_currentFrame.data[1] = byte;
|
|
_state = STATE_GOT_HEADER2;
|
|
} else {
|
|
// 不是 0x59, 重新开始
|
|
_errorCount++;
|
|
_state = STATE_IDLE;
|
|
if (byte == FRAME_HEADER_1) {
|
|
// 连续 0x53, 重新开始
|
|
_currentFrame.data[0] = byte;
|
|
_state = STATE_GOT_HEADER1;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case STATE_GOT_HEADER2:
|
|
// 读取控制字
|
|
_currentFrame.control = byte;
|
|
_state = STATE_GOT_CONTROL;
|
|
break;
|
|
|
|
case STATE_GOT_CONTROL:
|
|
// 读取命令字
|
|
_currentFrame.command = byte;
|
|
_state = STATE_GOT_COMMAND;
|
|
break;
|
|
|
|
case STATE_GOT_COMMAND:
|
|
// 读取长度高字节
|
|
_currentFrame.data_length = ((uint16_t)byte) << 8;
|
|
_state = STATE_GOT_LEN_H;
|
|
break;
|
|
|
|
case STATE_GOT_LEN_H:
|
|
// 读取长度低字节
|
|
_currentFrame.data_length |= byte;
|
|
|
|
// 长度安全检查
|
|
if (_currentFrame.data_length > RADAR_FRAME_BUF_SIZE) {
|
|
Serial.printf("[雷达] 帧数据长度异常: %d\n",
|
|
_currentFrame.data_length);
|
|
_errorCount++;
|
|
_state = STATE_IDLE;
|
|
return;
|
|
}
|
|
|
|
_dataIndex = 0;
|
|
|
|
// 如果数据长度为 0, 直接跳到校验状态
|
|
if (_currentFrame.data_length == 0) {
|
|
_state = STATE_GOT_DATA;
|
|
} else {
|
|
_state = STATE_GOT_LEN_L;
|
|
}
|
|
break;
|
|
|
|
case STATE_GOT_LEN_L:
|
|
// 读取数据字节
|
|
_currentFrame.data[_dataIndex++] = byte;
|
|
|
|
if (_dataIndex >= _currentFrame.data_length) {
|
|
_state = STATE_GOT_DATA;
|
|
}
|
|
break;
|
|
|
|
case STATE_GOT_DATA:
|
|
// 读取校验码
|
|
_currentFrame.checksum = byte;
|
|
_state = STATE_GOT_CHECKSUM;
|
|
break;
|
|
|
|
case STATE_GOT_CHECKSUM:
|
|
// 等待帧尾第一个字节 0x54
|
|
if (byte == FRAME_TAIL_1) {
|
|
_state = STATE_GOT_TAIL1;
|
|
} else {
|
|
_errorCount++;
|
|
_state = STATE_IDLE;
|
|
}
|
|
break;
|
|
|
|
case STATE_GOT_TAIL1:
|
|
// 等待帧尾第二个字节 0x43
|
|
if (byte == FRAME_TAIL_2) {
|
|
_state = STATE_FRAME_COMPLETE;
|
|
_currentFrame.timestamp = millis();
|
|
_processCompleteFrame();
|
|
_state = STATE_IDLE;
|
|
} else {
|
|
_errorCount++;
|
|
_state = STATE_IDLE;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
_state = STATE_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// 帧处理
|
|
// ============================================================
|
|
|
|
void RadarManager::_processCompleteFrame() {
|
|
_frameCount++;
|
|
|
|
// 校验
|
|
if (!_validateChecksum()) {
|
|
return;
|
|
}
|
|
|
|
_currentFrame.valid = true;
|
|
|
|
// 根据控制字提取数据
|
|
switch (_currentFrame.control) {
|
|
case CTRL_PRESENCE:
|
|
_extractPresence(_currentFrame);
|
|
break;
|
|
case CTRL_BREATHING:
|
|
_extractBreathing(_currentFrame);
|
|
break;
|
|
case CTRL_HEART_RATE:
|
|
_extractHeartRate(_currentFrame);
|
|
break;
|
|
case CTRL_FALL:
|
|
#if RADAR_TYPE == RADAR_TYPE_AFD1
|
|
_extractFall(_currentFrame); // AFD1 专用跌倒
|
|
#else
|
|
// ABD1 不会发 0x83 帧, 若收到可能是干扰, 直接丢弃
|
|
(void)_currentFrame;
|
|
#endif
|
|
break;
|
|
break;
|
|
case CTRL_SLEEP:
|
|
_extractSleep(_currentFrame);
|
|
break;
|
|
case CTRL_WORK_STATUS:
|
|
_extractWorkStatus(_currentFrame);
|
|
break;
|
|
case CTRL_DETECT_RANGE:
|
|
_extractDetectRange(_currentFrame);
|
|
break;
|
|
default:
|
|
Serial.printf("[雷达] 未识别控制字=0x%02X 长度=%d\n",
|
|
_currentFrame.control, _currentFrame.data_length);
|
|
// 打印数据内容辅助分析
|
|
Serial.print(" 数据: ");
|
|
for (int i = 0; i < _currentFrame.data_length && i < 16; i++) {
|
|
Serial.printf("%02X ", _currentFrame.data[i]);
|
|
}
|
|
Serial.println();
|
|
break;
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// 数据提取函数
|
|
// ============================================================
|
|
|
|
void RadarManager::_extractPresence(const RadarFrame& frame) {
|
|
uint8_t raw = frame.data_length > 0 ? frame.data[0] : 0;
|
|
bool rawPresence = false;
|
|
|
|
// cmd 0x00 (len=1): 有人状态 (1=有人 0=无人)
|
|
if (frame.command == 0x00 && frame.data_length >= 1) {
|
|
rawPresence = (raw == 1);
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
}
|
|
// cmd 0x01 (len=1): 运动状态/体动 (值越大越活跃)
|
|
else if (frame.command == 0x01 && frame.data_length >= 1) {
|
|
_data.body = raw;
|
|
bool rawMotion = (raw >= 1);
|
|
_filterMotion(rawMotion); // 3 帧防抖
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
return; // 运动帧不含有人信息, 跳过防抖
|
|
}
|
|
// cmd 0x03 (len=1): 有人状态备用 (非0=有人)
|
|
else if (frame.command == 0x03 && frame.data_length >= 1) {
|
|
rawPresence = (raw != 0);
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
}
|
|
// cmd 0x04 (len=2): [0]=presence [1]=motion
|
|
else if (frame.command == 0x04 && frame.data_length >= 2) {
|
|
rawPresence = (frame.data[0] != 0);
|
|
bool rawMotion = (frame.data[1] == 0);
|
|
_filterMotion(rawMotion); // 3 帧防抖
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
}
|
|
// cmd 0x05 (len=6): 完整包
|
|
else if (frame.command == 0x05 && frame.data_length >= 6) {
|
|
rawPresence = (frame.data[0] != 0);
|
|
bool rawMotion = (frame.data[1] == 0);
|
|
_filterMotion(rawMotion); // 3 帧防抖
|
|
_data.distance = ((uint16_t)frame.data[2] << 8) | frame.data[3];
|
|
_data.body = frame.data[4];
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
} else {
|
|
return; // 未知命令
|
|
}
|
|
|
|
// 防抖: 连续 PRESENCE_HYSTERESIS 次一致才更新
|
|
if (rawPresence == _lastRawPresence) {
|
|
if (_presenceCnt < PRESENCE_HYSTERESIS) _presenceCnt++;
|
|
} else {
|
|
_presenceCnt = 0;
|
|
_lastRawPresence = rawPresence;
|
|
}
|
|
if (_presenceCnt >= PRESENCE_HYSTERESIS && rawPresence != _data.presence) {
|
|
_data.presence = rawPresence;
|
|
_stablePresence = rawPresence;
|
|
}
|
|
_notifyUpdate();
|
|
}
|
|
|
|
void RadarManager::_extractBreathing(const RadarFrame& frame) {
|
|
if (frame.data_length >= 1) {
|
|
uint8_t rate = frame.data[0];
|
|
if (rate != 0x80 && rate != 0x00 && rate <= 40) {
|
|
_data.breathRate = rate;
|
|
_data.vitalValid = true;
|
|
_data.presence = true;
|
|
_data.radarValid = true;
|
|
_lastBreathBeat = millis();
|
|
}
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
}
|
|
}
|
|
|
|
void RadarManager::_extractHeartRate(const RadarFrame& frame) {
|
|
if (frame.data_length >= 1) {
|
|
uint8_t rate = frame.data[0];
|
|
if (rate != 0x80 && rate != 0x00 && rate >= 40) {
|
|
_data.heartRate = rate;
|
|
_data.vitalValid = true;
|
|
_data.presence = true;
|
|
_data.radarValid = true;
|
|
_lastHeartBeat = millis();
|
|
}
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
}
|
|
}
|
|
|
|
// AFD1 专用: 跌倒检测 (控制字 0x83)
|
|
void RadarManager::_extractFall(const RadarFrame& frame) {
|
|
if (frame.data_length >= 1) {
|
|
_data.fall = (frame.data[0] != 0);
|
|
_data.radarValid = true;
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
}
|
|
}
|
|
|
|
void RadarManager::_extractSleep(const RadarFrame& frame) {
|
|
// 从睡眠数据提取: cmd=0x00/0x01 为入床离床状态
|
|
if (frame.data_length >= 1) {
|
|
// data[0]: 0=离床/无驻留, 非0=入床/驻留
|
|
if (frame.command == 0x00 || frame.command == 0x01) {
|
|
_data.resident = (frame.data[0] != 0);
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
}
|
|
}
|
|
|
|
if (frame.data_length >= 3) {
|
|
_data.sleepState = frame.data[1]; // 0=清醒 1=浅睡 2=深睡
|
|
_data.sleepScore = frame.data[2]; // 0~100
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
}
|
|
|
|
if (frame.data_length >= 4) {
|
|
#if RADAR_TYPE == RADAR_TYPE_AFD1
|
|
// AFD1 协议: data[3] 低 2 位 = 跌倒/驻留
|
|
_data.fall = (frame.data[3] & 0x01) != 0;
|
|
_data.resident = (frame.data[3] & 0x02) != 0;
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
#else
|
|
// ABD1 协议: data[3] bit0 是睡眠状态标志 (不是跌倒), bit1 才是驻留
|
|
// 之前误把 bit0 当 fall 触发误报, 现只取 resident
|
|
_data.resident = (frame.data[3] & 0x02) != 0;
|
|
_data.lastUpdate = millis();
|
|
_notifyUpdate();
|
|
#endif
|
|
}
|
|
}
|
|
|
|
// 运动状态防抖: 连续 3 帧一致才更新 _data.motion
|
|
// 雷达体动信号容易因小幅抖动/呼吸胸腔起伏/电磁干扰在 0/1 之间跳变
|
|
void RadarManager::_filterMotion(bool rawMotion) {
|
|
if (rawMotion == _lastRawMotion) {
|
|
if (_motionCnt < 3) _motionCnt++;
|
|
} else {
|
|
_motionCnt = 0;
|
|
_lastRawMotion = rawMotion;
|
|
}
|
|
if (_motionCnt >= 3 && rawMotion != _data.motion) {
|
|
_data.motion = rawMotion;
|
|
}
|
|
}
|
|
|
|
void RadarManager::_extractWorkStatus(const RadarFrame& frame) {
|
|
if (frame.command == WS_INIT_COMPLETE && frame.data_length >= 1) {
|
|
if (frame.data[0] == 0x0F) {
|
|
_initialized = true;
|
|
Serial.println(F("[雷达] ✅ 雷达初始化完成"));
|
|
}
|
|
}
|
|
}
|
|
|
|
void RadarManager::_extractDetectRange(const RadarFrame& frame) {
|
|
// 探测范围信息
|
|
if (frame.data_length >= 1) {
|
|
Serial.printf("[雷达] 探测范围状态: 0x%02X\n", frame.data[0]);
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// 校验码计算 & 验证
|
|
// ============================================================
|
|
|
|
uint8_t RadarManager::_calcChecksum(const uint8_t* data, uint16_t len) {
|
|
uint16_t sum = 0;
|
|
for (uint16_t i = 0; i < len; i++) {
|
|
sum += data[i];
|
|
}
|
|
return (uint8_t)(sum & 0xFF);
|
|
}
|
|
|
|
bool RadarManager::_validateChecksum() {
|
|
// 计算: 帧头(2) + 控制字(1) + 命令字(1) + 长度(2) + 数据(N) 求和取低8位
|
|
uint8_t header[6];
|
|
header[0] = FRAME_HEADER_1;
|
|
header[1] = FRAME_HEADER_2;
|
|
header[2] = _currentFrame.control;
|
|
header[3] = _currentFrame.command;
|
|
header[4] = (_currentFrame.data_length >> 8) & 0xFF;
|
|
header[5] = _currentFrame.data_length & 0xFF;
|
|
|
|
uint16_t sum = 0;
|
|
for (int i = 0; i < 6; i++) sum += header[i];
|
|
for (uint16_t i = 0; i < _currentFrame.data_length; i++) sum += _currentFrame.data[i];
|
|
|
|
uint8_t expected = (uint8_t)(sum & 0xFF);
|
|
return (expected == _currentFrame.checksum);
|
|
}
|
|
|
|
// ============================================================
|
|
// 解析器重置
|
|
// ============================================================
|
|
|
|
void RadarManager::_resetParser() {
|
|
_state = STATE_IDLE;
|
|
_dataIndex = 0;
|
|
memset(&_currentFrame, 0, sizeof(_currentFrame));
|
|
}
|
|
|
|
// ============================================================
|
|
// 获取最新数据 (线程安全版本)
|
|
// ============================================================
|
|
|
|
RadarRawData RadarManager::getData() const {
|
|
return _data;
|
|
}
|
|
|
|
// ============================================================
|
|
// 回调注册
|
|
// ============================================================
|
|
|
|
void RadarManager::onDataUpdate(RadarCallback cb) {
|
|
_callback = cb;
|
|
}
|
|
|
|
void RadarManager::_notifyUpdate() {
|
|
if (_callback) {
|
|
_callback(_data);
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// 发送命令到雷达
|
|
// ============================================================
|
|
|
|
void RadarManager::sendCommand(uint8_t control, uint8_t command,
|
|
const uint8_t* data, uint16_t len) {
|
|
if (!_serial) return;
|
|
|
|
// 构建帧
|
|
uint8_t frame[RADAR_FRAME_BUF_SIZE + 9];
|
|
uint16_t idx = 0;
|
|
|
|
frame[idx++] = FRAME_HEADER_1;
|
|
frame[idx++] = FRAME_HEADER_2;
|
|
frame[idx++] = control;
|
|
frame[idx++] = command;
|
|
frame[idx++] = (len >> 8) & 0xFF;
|
|
frame[idx++] = len & 0xFF;
|
|
|
|
if (data && len > 0) {
|
|
memcpy(&frame[idx], data, len);
|
|
idx += len;
|
|
}
|
|
|
|
// 校验: 帧头(2) + 控制(1) + 命令(1) + 长度(2) + 数据(N)
|
|
uint16_t sum = 0;
|
|
for (uint16_t i = 0; i < idx; i++) sum += frame[i];
|
|
frame[idx++] = (uint8_t)(sum & 0xFF);
|
|
|
|
frame[idx++] = FRAME_TAIL_1;
|
|
frame[idx++] = FRAME_TAIL_2;
|
|
|
|
_serial->write(frame, idx);
|
|
_serial->flush();
|
|
}
|
|
|
|
void RadarManager::queryInitStatus() {
|
|
// 查询初始化状态: 控制字 0x05, 命令字 0x81, 数据 0x0F
|
|
uint8_t d = 0x0F;
|
|
sendCommand(CTRL_WORK_STATUS, WS_INIT_QUERY, &d, 1);
|
|
}
|
|
|
|
void RadarManager::queryDetectRange() {
|
|
sendCommand(CTRL_DETECT_RANGE, CMD_QUERY);
|
|
}
|