From 8d8e874de93c823372c7ba0ed78d1e92c2cfc54c Mon Sep 17 00:00:00 2001 From: cxh Date: Fri, 6 Jun 2025 15:39:58 +0800 Subject: [PATCH] Auto commit at 2025-06-06 15:39:58 --- src/radio_ctrl/src/uart_handler.cpp | 30 ++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/src/radio_ctrl/src/uart_handler.cpp b/src/radio_ctrl/src/uart_handler.cpp index 11d44bc..76fba1e 100644 --- a/src/radio_ctrl/src/uart_handler.cpp +++ b/src/radio_ctrl/src/uart_handler.cpp @@ -143,31 +143,39 @@ void UartHandler::parse_data(std::vector &buffer) { while (buffer.size() >= kSbusFrameLength) { - // 寻找帧头（SBUS 通常帧头是 0x0F） + // 找到帧头 0x0F auto it = std::find(buffer.begin(), buffer.end(), 0x0F); if (it == buffer.end()) { - buffer.clear(); // 无帧头，清空 + buffer.clear(); // 没找到帧头，清空 buffer failsafe_status = SBUS_SIGNAL_LOST; return; } - // 如果帧头之后数据不够 25 字节，等待下次接收 + // 计算帧头位置 size_t index = std::distance(buffer.begin(), it); + + // 判断是否有足够长度 if (buffer.size() - index < kSbusFrameLength) { - // 保留还没凑够的部分 + // 数据不够，等下次再处理 if (index > 0) - buffer.erase(buffer.begin(), buffer.begin() + index); // 移除前面无用部分 + buffer.erase(buffer.begin(), buffer.begin() + index); return; } - // 拷贝并解析数据 - std::memcpy(sbus_data, &buffer[index], kSbusFrameLength); - sbus_parse(); - - // 移除已处理数据 - buffer.erase(buffer.begin(), buffer.begin() + index + kSbusFrameLength); + // 判断帧尾是否为 0x00 + if (buffer[index] == 0x0F && buffer[index + 24] == 0x00) + { + std::memcpy(sbus_data, &buffer[index], kSbusFrameLength); + sbus_parse(); + buffer.erase(buffer.begin(), buffer.begin() + index + kSbusFrameLength); + } + else + { + // 帧头是 0x0F 但帧尾不对，跳过当前帧头，继续寻找下一帧 + buffer.erase(buffer.begin(), buffer.begin() + index + 1); + } } }