Auto commit at 2025-06-06 15:39:58

src/radio_ctrl/src/uart_handler.cpp

@@ -143,31 +143,39 @@ void UartHandler::parse_data(std::vector<uint8_t> &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;
         }
 
-        // 拷贝并解析数据
+        // 判断帧尾是否为 0x00
-        std::memcpy(sbus_data, &buffer[index], kSbusFrameLength);
+        if (buffer[index] == 0x0F && buffer[index + 24] == 0x00)
-        sbus_parse();
+        {
-
+            std::memcpy(sbus_data, &buffer[index], kSbusFrameLength);
-        // 移除已处理数据
+            sbus_parse();
-        buffer.erase(buffer.begin(), buffer.begin() + index + kSbusFrameLength);
+            buffer.erase(buffer.begin(), buffer.begin() + index + kSbusFrameLength);
+        }
+        else
+        {
+            // 帧头是 0x0F 但帧尾不对，跳过当前帧头，继续寻找下一帧
+            buffer.erase(buffer.begin(), buffer.begin() + index + 1);
+        }
     }
 }