feat: init

2026-07-22 11:27:37 +08:00
commit 49e3abbb34
23 changed files with 4388 additions and 0 deletions
@@ -0,0 +1,473 @@
+/**
+ * @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)
+{
+    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();
+
+    // 不给雷达发任何命令，让它自己初始化并开始自动上报
+    // 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 秒还没收到任何数据，打印一次
+    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_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;
+
+    // cmd 0x00 (len=1): 有人状态 (1=有人 0=无人)
+    if (frame.command == 0x00 && frame.data_length >= 1) {
+        _data.presence   = (raw == 1);
+        _data.radarValid = true;
+        _data.lastUpdate = millis();
+        _notifyUpdate();
+    }
+    // cmd 0x01 (len=1): 运动状态/体动 (值越大越活跃)
+    else if (frame.command == 0x01 && frame.data_length >= 1) {
+        _data.body       = raw;         // 0=静止, 1+=有体动
+        _data.motion     = (raw >= 1);  // >=1 有运动
+        _data.radarValid = true;
+        _data.lastUpdate = millis();
+        _notifyUpdate();
+    }
+    // cmd 0x03 (len=1): 有人状态备用 (非0=有人)
+    else if (frame.command == 0x03 && frame.data_length >= 1) {
+        _data.presence   = (raw != 0);
+        _data.radarValid = true;
+        _data.lastUpdate = millis();
+        _notifyUpdate();
+    }
+    // cmd 0x04 (len=2): [0]=presence [1]=motion
+    else if (frame.command == 0x04 && frame.data_length >= 2) {
+        _data.presence   = (frame.data[0] != 0);
+        _data.motion     = (frame.data[1] == 0);
+        _data.radarValid = true;
+        _data.lastUpdate = millis();
+        _notifyUpdate();
+    }
+    // cmd 0x05 (len=6): 完整包
+    else if (frame.command == 0x05 && frame.data_length >= 6) {
+        _data.presence   = (frame.data[0] != 0);
+        _data.motion     = (frame.data[1] == 0);
+        _data.distance   = ((uint16_t)frame.data[2] << 8) | frame.data[3];
+        _data.body       = frame.data[4];
+        _data.radarValid = true;
+        _data.lastUpdate = millis();
+        _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;
+        }
+        _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;
+        }
+        _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[0]: 入床/离床
+        // data[1]: 睡眠状态
+        // data[2]: 评分
+
+        // 跌倒检测: 可能在睡眠异常标志中
+        if (frame.data_length >= 4) {
+            _data.fall     = (frame.data[3] & 0x01) != 0;
+            _data.resident = (frame.data[3] & 0x02) != 0;
+        }
+
+        _data.lastUpdate = millis();
+        _notifyUpdate();
+    }
+}
+
+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);
+}